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I.    INTRODUCTION 

The  study  of  robotics  is  a  fairly  new  area  in  the  fields  of  science 
and  engineering  which  has  fundamentals  that  can  be  traced  back  to 
many  basic  disciplines,  such  as  electrical  engineering,  mechanical 
engineering,  and  computer  science.  The  mechanical  engineering 
interest  in  the  study  of  robotic  manipulators  can  be  traced  back  to  as 
early  as  the  1940s,  although  the  first  industrial  robots  were  not  intro- 
duced until  the  late  1950s  and  early  1960s  [Ref.  1].  Mechanical  engi- 
neers were  mostly  interested  in  two  areas  of  robotics:  the  control  of 
the  linkage  and  the  study  of  its  motion.  While  these  two  areas  are 
interrelated,  this  thesis  focuses  on  the  second:  the  study  of  the  motion 
of  the  linkage. 

The  study  of  robot  motion  (dynamics)  is  further  divided  into  the 
study  of  arm  dynamics  and  robot  arm  kinematics.  Both  these  divisions 
are  subdivided  into  a  direct  and  an  inverse  problem.  The  direct 
dynamics  problem  is  the  calculation  of  link  state  variables  such  as 
acceleration,  velocity,  and  joint  angles  from  the  known  joint  torques. 
The  inverse  dynamic  problem  assumes  state  variables  are  known  and 
the  joint  torques  are  to  be  calculated.  In  the  kinematics,  the  direct 
problem  is  the  determination  of  the  end  effector  position  and  its 
orientation  from  a  given  set  of  link  geometries  and  joint  angles.  The 
inverse  problem  is  the  calculation  of  the  link  geometries  and  joint 
angles  for  a  given  end  effector  position. 


Various  methods  to  solve  the  inverse  kinematic  problem  use 
matrix  transformations,  inversion,  and  multiplication,  and  as  such  are 
subjected  to  singularity  problems  [Ref.  2].  Singularity  occurs  when  two 
successive  links  are  aligned  (i.e.,  the  angle  between  the  links  is  0  or 
180  degrees).  When  a  singularity  occurs,  the  matrices  involved  are  also 
singular  and  not  invertible. 

A  method  to  avoid  the  singularity  problem  was  developed  at  the 
Naval  Postgraduate  School  using  Newtonian  dynamics  in  terms  of  a 
globally  fixed  coordinate  system  [Ref.  3].  This  method  treats  each  link 
as  a  free  body  with  the  forces  and  moments  applied  at  the  joints  and 
the  use  of  Newton's  law  to  derive  the  equations  of  motion.  In  this 
thesis,  the  development  of  the  free  body  analysis  approach  to  rigid 
manipulators  dynamics  is  continued  and  extended  to  three  dimen- 
sions and  including  gravitational  effects. 


II.     PROBLEM  STATEMENT 

Theoretical  dynamic  approaches  are  well  developed  and  have 
been  extensively  used  with  simulations.  However,  in  manipulator 
studies,  these  accepted  methods  all  have  a  singularity  that  arises  when 
two  successive  links  are  aligned.  This  does  not  allow  the  motion  to  be 
simulated  [Ref.  31 .  This  shortfall  cannot  be  tolerated  in  the  modelling 
of  an  actual  robot  or  robotic  manipulator  because  it  is  of  the  utmost 
importance  to  accurately  know  the  arm  position  at  all  times  (for  real- 
time control  purposes)  [Ref.  2].  Therefore,  the  problem  directed  by 
this  thesis  research  project  was: 

Continue  the  development  of  the  Newton  Euler  free-body  approach 
for  a  three-link  manipulator  extended  to  three-dimensional  motion 
and  including  gravitational  effects.  In  addition,  compare  simulation 
results  with  those  of  an  actual  robotic  arm.  [Ref.  4] 


III.  BACKGROUND 

A.     THEORY 

The  basic  theory  for  the  Newton  Euler  free-body  analysis  to  over- 
come the  singularity  problems  of  conventional  robot  dynamics  was 
developed  by  Sadrettin  Altinok  [Ref.  3].  The  theory  of  treating  each 
link  of  the  manipulator  as  a  free  body  with  respect  to  a  global  coordi- 
nate system  leads  to  the  following  27  equations  for  a  three-link 
manipulator. 

FXO  +  M1*AX1  -  FX1  =  0  ( 1 ) 

FY0  + M1*AY1  -FY1  =  0  (2) 

FZO  +  M1*AZ1  -  FZ1  =  -WG1  (3) 

AX1  +  RB1Z*WD1Y  -  RB1X*WD1Z  =  AXO  -  MICO  (4) 

where  MICO  =  (X  component  of) 

WD1  X  RBG1  +  Wl  X  fWl  X  RBG11  (4a) 

AY1  -  RB1Z*WD1X  +  RB1X*WD1Z  =  AYO  -  MJCO  (5) 

where  MJCO  =  (Y  component  of  equation  4a). 


AZ1  +  RBY1*WD1X  -RB1Z*WD1Y  =  AZO  -  MKCO  (6) 

where  MKCO  =  (Z  component  of  equation  4a). 

RB1Z*FY0  -  RB1Y*FZ0  -  IXXT1*WD1X  +  IXYT1*WD1Y  + 

IXZT1*WD1Z  -  RA1Z*FY1  +  RA1Y*FZ1  =  T1X  -  TOX  (7) 

-RB1Z*FX0  +  RB1X*FZ0  +  IXYT1*WD1X  -  IYYT1*WD1Y  + 

IYZT1*WD1Z  +  RA1Z*FX1  -  RA1X*FZ1  =  T1Y  -  TOY  (8) 

RB1Y*FX0  -  RB1Z*FY0  +  IXZT1*WD1X  +  IYZT1*WD1Y  - 

IZZT1*WD1Z  -  RA1Y*FX1  +  RA1X*FY1  =  T1Z  -  TOZ  (9) 

FX1  +  M2*AX2  -  FX2  =  0  (10) 

FY1  +  M2*AY2  -  FY2  =  0  (11) 

FZ1  +  M2*AZ2  -  FZ2  =  -WG2  (12) 

-AX1  -  RA1Z*WD1Y  +  RA1Y*WD1Z  +  AX2  +  RB2Z*WD2Y  - 

RB2Y*WD2Z  =  MIC1  -  MIC2  ( 1 3) 

where  MIC1  =  (X  component  of) 

WD1  X  RA1  +  Wl  X  fWl  X  RAD  (13a) 


where  MIC2  +  (X  component  of) 

WD2  X  RB2  +  W2  X  fW2  X  RB2)  (13b) 

-AY1  +  RA1Z*WD1X  -  RA1X*WD1Z  +  AY2  -  RB2Z*WD2X  + 

RB2X*WD2Z  =  MJC 1  -  MJC2  ( 1 4) 

where  MJC1  =  (Y  component  of  equation  13a)  and  where  MJC2  =  (Y 
component  of  equation  13b). 

-AZ1  -  RA1Y*WD1X  +  RA1X*WD1Y  +  AZ2  +  RB2Y*WD2X  - 

RB2X*WD2Y  =  MKC 1  -  MKC2  (15) 

where  MKC1  =  (Z  component  of  equation  13a)  and  where  MKC2  =  (Z 
component  of  equation  13b). 

RB2Z*FY1  -  RB2Y*FZ1  -  IXXT2*WD2X  +  IXYT2*WD2Y  + 

IXZT2*WD2Z  -  RA2Z*FY2  +  RA2Y*FZ2  =  T2X  -  T1X  ( 1  6) 


-RB2Z*FX1  +  RB2X*FZ1  +  IXYT2*WD2X  -  IYYT2*WD2Y  + 

IYZT2*WD2Z  +  RA2Z*FX2  -  RA2X*FZ2  =  T2Y  -  T1Y  ( 1 7) 

RB2Y*FX1  +  RB2X*FY1  +  IXZT2*WD2X  +  IYZT2*WD2Y  - 

IZZT2*WD2Z  -  RA2Y*FX2  +  RA2X*FY2  =  T2Z  -  T1Z  (18) 


FX2  +  M3*AX3  =  0  (19) 

FY2  +  M3*AY3  =  0  (20) 

FZ2  +  M3*AZ3  =  -WG3  (21) 

-AX2  -  RA2Z*WD2Y  +  RA2Y*WD2Z  +  AX3  +  RB3Z*WD3Y  - 

RB3Y*WD3Z  =  MIC3  -  MIC4  (22) 

where  MIC3  =  (X  component  of) 

WD2  X  RA2  +  W2  X  fW2  X  RA2)  (22a) 

where  MIC4  =  (X  component  of) 

WD3  X  RB3  +W3X  fW3  X  RB3)  (22b) 

-AY2  +  RA2Z*WD2X  -  RA2X*WD2Z  +  AY3  -  RB3Z*WD3X  + 

RB3X*WD3Z  =  MJC3  -  MJC4  (23) 

where  MJC3  =  (Y  component  of  equation  22a)  and  where  MJC4  =  (Y 
component  of  equation  22b). 

-AZ2  -  RA2Y*WD2X  +  RA2X*WD2Z  +  AZ3  +  RB3Y*WD3X  - 

RB3X*WD3Y  =  MKC3  -  MKC4  (24) 


where  MKC3  =  (Z  component  of  equation  22a)  and  where  MKC4  =  (Z 
component  of  equation  22b). 

RB3Z*FY2  -  RB3Y*FZ2  -  IXXT3*WD3X  +  IXYT3*WD3Y  + 

IXZT3*WD3Z  =  -  T2X  (25) 

-RB3Z*FX2  +  RB3X*FZ2  +  IXYT3*WD3X  -  IYYT3*WD3Y  + 

IYZT3*WD3Z  =  -  T2Y  (26) 

RB3Y*FX2  -  RB3X*FY2  +  IXZT3*WD3X  +  IYZT3*WD3Y  - 

IZZT3*WD3Z  =  -  T2Z  (27) 

B.      COMPUTATION 

The  Dynamic  Simulation  Language  (DSL)  was  used  to  simulate  the 
direct  dynamics  problem.  A  matrix  (MATA,  a  27  x  27  matrix)  was  cre- 
ated from  the  known  coefficients  of  the  unknown  variables  in  the  pre- 
vious 27  equations.  This  was  possible  due  to  the  assumption  that  the 
change  in  the  inertia  of  the  links,  the  link  velocities,  and  the  joint 
position  must  be  extremely  small  during  a  simulated  step  interval  and 
can  thus  be  treated  as  being  constant  during  the  step.  In  the  direct 
dynamics  problem,  a  vector  (MatB,  27  x  1)  was  generated  from  the 
following  components:  joint  torques,  the  centers  of  gravity  of  the  link, 
and  the  calculated  cross-products  of  angular  velocities  (Figure  1).  This 
gave  a  system  of  equations  in  the  form  of  equation  28: 
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Figure  1.    Matrix  Entries  for  Simulation 


A*x  =  b  (28) 

where  A  and  b  are  known  and  x  is  an  unknown  vector  (which  contains 
the  forces  at  the  link  ends  and  the  angular  and  linear  accelerations  of 
each  link).  The  IMSL  routine  LEQT2F  (a  linear  equation  solver)  was 
used  to  obtained  the  vector  x.  [Ref.  3  and  Ref.  5] 

In  the  inverse  problem,  MATA  was  developed  the  same  as  in  the 
direct  problem  as  discussed  above,  but  now  vector  x  was  known  (from 
the  history  of  position  data)  and  vector  b  was  found  by  straight  matrix 
multiplication  of  equation  28.  [Ref.  3  and  Ref.  5] 

The  previous  work  used  transformation  matrix  operations  to 
obtain  joint  orientations  [Ref.  3].  This  was  done  because  joint  orienta- 
tion cannot  be  obtained  directly  from  the  integration  of  rotational 
velocity  [Ref.  6  and  Ref.  7].  The  approach  used  in  this  thesis  was  one  of 
simple  geometric  relationships.  It  was  reasoned  that  the  linear  accel- 
erations were  either  known  or  calculated  from  the  matrix  operation  of 
LEQT2F  (equation  28).  These  linear  accelerations  were  the  linear 
accelerations  of  the  link  center  of  gravities  with  respect  to  the  earth 
fixed  (inertial)  coordinate  system  which  were  integrated  to  obtain  the 
linear  velocities.  The  velocities  were  further  integrated  to  obtain  the 
position  of  the  center  of  gravities. 

Knowing  the  earth  fixed  coordinates  of  the  centers  of  gravity,  and 
that  each  link  can  be  represented  by  a  rigid  body  [Ref.  81,  it  was 
known  that  the  center  of  gravity  must  be  on  the  vector  from  the 
inboard  joint  to  the  end  of  the  link,  or  next  joint  (Figure  2).  Once  the 
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Figure  2.   Link  Geometries 
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vector  was  known  to  the  center  of  gravity,  the  directional  cosines 
were  calculated  from  the  following  three  equations  for  each  link. 

DRCOSX  =  [LCOGX  -  JX(inboard)]/LNCOG  (29) 

DRCOSY  =  [LCOGY  -  JY(inboard)]/LNCOG  (30) 

DRCOSZ  =  [LCOGZ  -  JZ(inboard)]/LNCOG  (3 1 ) 

where  LCOGX  is  the  X  location  of  the  CG  with  respect  to  the  inertia 
frame;  JX  is  the  X  location  of  the  inboard  joint  location  with  respect  to 
the  inertia;  and  LNCOG  is  magnitude  of  the  length  from  the  inboard 
joint  to  the  CG. 

Once  the  directional  cosines  were  known,  the  same  method  as 
used  by  previous  investigators  was  used  to  calculate  the  MATA  and 
MATB  matrices. 

C.     CONSTRAINTS 

With  the  desire  to  simulate  the  motion  of  an  actual  robotic  manip- 
ulator and  to  compare  the  simulation  with  data  from  the  manipulator, 
constraints  had  to  be  added  into  the  simulation  so  the  simulation 
would  take  into  account  the  the  joint  mechanical  restrictions.  In  the- 
ory, each  joint  is  a  one  degree  of  freedom  connection.  However,  in  the 
chosen  arm,  the  zero  joint  rotates  in  the  Z  direction  only,  while  joints 
one  and  two  rotate  about  both  the  X  and  Y  axes  (Figure  3).  The  previ- 
ously mentioned  constraints  led  to  the  following  equations: 
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Figure  3.   Robot  Axis  Rotation  with  Constraints 
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AX1  =  0  (32) 

AY1  =  0  (33) 

AZ1  =  0  (34) 

WDX1=0  (35) 

WDY1  =  0  (36) 

WDZ1  =  WDZ2  =  WDZ3  (37) 

The  concept  for  entering  the  constraints  into  the  simulation  was 
developed  in  Reference  3  and  led  to  the  constraint  section  of  the  DSL 
code  as  seen  in  Appendix  A  and  Appendix  B.  The  basic  concept  which 
was  developed  was  to  zero  out  the  row  only  in  MATA,  then  set  the 
diagonal  of  that  row  to  one  and  the  corresponding  row  of  MATB  to 
zero.  This  concept  was  changed  to  include  zeroing  out  the  row  and 
column  of  MATA  corresponding  to  the  row  of  vector  x,  which  was  to 
be  zero,  along  with  setting  the  diagonal  of  the  row  to  1  and  the  corre- 
sponding row  of  MATB  to  zero. 

D.     GRAVITATIONAL  TORQUES 

The  simulation  of  gravitational  effects  on  the  arm  required  that  an 
equalizing  torque  be  applied  at  the  joints  to  overcome  the  gravitational 
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D.     GRAVITATIONAL  TORQUES 

The  simulation  of  gravitational  effects  on  the  arm  required  that  an 
equalizing  torque  be  applied  at  the  joints  to  overcome  the  gravitational 
pull  (Figure  4).  This  torque  varied  as  the  Z  location  of  the  center  of 
gravity  of  links  2  and  3  varied  because  the  magnitude  of  the  moment 
arm  varied  as  the  link  moved.  The  lever  arm  which  affected  the  gravi- 
tational torque  was  the  projection  of  the  vector  from  the  inboard  joint 
to  the  link  center  of  gravity  onto  the  XY  plane  (Figure  5).  Knowing  this 
lever  arm  and  the  weight  of  the  links,  the  following  equations  were 
used  to  calculate  the  equilibrium  torque  needed  to  overcome  gravity: 

MARM2A  =  SQRT  (LCOGX2*LCOGX2  +  LCOGY2*LCOGY2)  (38) 

MARM2B  =  SQRT  (LCOGX3*LCOGX3  +  LCOGY3*LCOGY3)  (39) 

MARM3  =  SQRT  ((LCOGX3^JX2)**2  +  (LCOGY3^JY2)**2)  (40) 

TGI  =  MAR2A  *  WG2  +  MARM2B  *  WG3  (41) 

TG2  =  MARM3  *  WG3  (42) 

The  equations  above  represent  the  magnitude  of  the  required 
torque  which  must  be  resolved  into  its  X  and  Y  components  (Figure  6). 
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Figure  4.    Gravitational  Torques 
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Figure  5.    Gravitational  Torque  Moment  Arms 
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Figure  6.    Gravitational  Torque  X  and  Y  Components 
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With  the  given  constraints,  this  was  done  in  the  following  manner. 
Angle  THZ  was  calculated  from  the  second  integral  of  WDZ1  (the  base 
link  only  revolved  around  its  vertical  axis).  Then  its  cosine  and  sine 
were  used  to  obtain  the  X  and  Y  components  of  the  gravitational  equi- 
librium torque  as  follows: 

TGX  =  TG  *  COS  (THZ)  (43) 

TGY  =  TG  *  SIN  (THZ)  (44) 

E.      INERTIAS 

The  inertias  of  the  links  of  the  robot  were  calculated  the  first 
time  from  the  equations  of  Reference  2  in  the  global  (inertial) 
reference  frame  and  then  transferred  to  the  local  coordinates  (Figure 
7)  by  the  use  of  a  transformation  [Ref.  9],  giving  the  following  equation: 

Inertia(local)  =  TMAT  *  Inertia(global)  *  TMATTR  (45) 

where  MATA  is  a  transformation  matrix  based  on  the  link  angles 
[Ref.  1  and  Ref.  10].  Knowing  that  the  local  inertias  remain  constant, 
the  global  inertias  for  each  link  were  calculated  for  each  time  iteration 
by  the  use  of  the  inverse  transformation  below: 

Inertia(global)  =  TMATTR  *  Inertia(local)  *  TMAT  (46) 
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Figure  7.   Inertia  Coordinate  Systems 
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IV.     SIMULATION  NONSINGULAR  MOTION  VALIDATION 

A.     TWO-DIMENSIONAL   MOTION  VALIDATION 

A  known  motion  which  had  adjacent  links  aligned  was  that  of  the 
two-dimensional  double  pendulum  (Figure  8).  A  comparison  of  the 
simulation  to  the  theoretical  solution  was  used  to  validate  the 
nonsingularity,  the  motion  tracking,  and  the  gravitational  effects  of  the 
simulation. 

The  theoretical  solution  [Ref.  11]  used  a  Lagrange  equation  which 
only  included  the  linear  kinetic  energy  of  the  system  (equation  47), 
while  the  simulation  included  both  the  linear  and  rotational  kinetic 
energies  (equation  48).  To  account  for  the  difference,  the  inertia  in 
equation  48  was  set  to  a  small  value.  Note  that  it  was  not  possible  to 
set  the  inertias  to  zero  because  the  inertia  entries  in  matrix  MATA 
were  on  the  diagonal  and  would  thus  not  have  allowed  the  inverse  to 
be  calculated. 

Kinetic  Energy  =  1/2  *  m  *  v2  (47) 

Kinetic  Energy  =  1/2  *  m  *  v2  +  1/2  I  *  w2  (48) 

The  point  mass  method  developed  in  Reference  3  allowed  the 
first  mass  of  the  double  pendulum  to  be  set  to  a  value  equal  to  that  of 
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Figure  8.   Double  Pendulum  Configuration 
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the  outer  point  mass  of  link  2  plus  the  inner  point  mass  of  link  3 
(equation  49).  The  second  mass  of  the  double  pendulum  was  set  to  the 
value  of  the  outer  point  mass  of  link  3  (equation  50). 

ml  (dp)  =  m  (2,2)  +  m  (3,1)  (49) 

m2  (dp)  =  m  (3.2)  (50) 

The  lengths  of  the  double  pendulum  were  the  same  as  the  link 
lengths  (Figure  8). 

The  results  of  the  theoretical  and  simulation  program  for  the 
double  pendulum  problem  agreed  (Figures  9  and  10),  and  would  have 
been  identical  had  the  inertia  been  set  to  zero.  Tracking  of  the  motion 
through  the  potential  singular  points  presented  no  difficulties  to  the 
simulation  (Figure  11). 

B.     THREE-DIMENSIONAL  NONSINGULAR   MOTION 

A  motion  which  had  potential  singular  points  was  chosen  which 
validated  the  nonsingularity  of  the  simulation  in  three  dimensions 
(Figure  12).  The  motion  which  was  chosen  was  to  input  the  joint  1  and 
joint  2  angles  as  a  time  function  and  allow  the  third  joint  angle  to 
remain  zero,  so  links  2  and  3  were  always  aligned  and  potentially 
singular.  This  chosen  motion  also  had  all  three  links  aligned  at  time 
equal  to  0.785  seconds  (Figure  12). 
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Figure  9.   Double  Pendulum  Angle  2 
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Figure  10.    Double  Pendulum  Angle  3 
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Figure  12.   Three-Dimensional  Nonsingular  Validation 
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The  simulation  again  had  no  difficulties  tracking  the  input  motion 
through  the  potentially  singular  points  (Figure  13).  The  error  in  the 
predicted  and  simulated  end  effector  position  (Figure  14)  was  never 
greater  than  1.0  percent. 
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V.     EXPERIMENTAL  VALIDATION 

A.  MANIPULATOR 

A  robot  manipulator  was  chosen  to  check  the  agreement  of  the 
simulation  to  data  from  an  actual  robot  arm.  The  arm  that  was  available 
was  the  NEPTUNE  II  (Figure  15),  which  is  a  low-performance, 
hydraulic-operated  arm.  This  arm  required  certain  hardware  modifi- 
cations, namely: 

•  the  remounting  of  the  electronic  solenoid  boards  to  facilitate 
access  and  provide  protection  from  hydraulic  leaks; 

•  installation  of  bleed  holes  in  the  major  joint  pistons  to  allow  air  to 
be  bled  off  when  the  system  has  been  opened  for  repairs; 

•  design  and  manufacture  of  a  pump  and  accumulator  stand  that 
would  provide  spray  protection  for  the  manipulator  in  the  case  of 
malfunctions; 

•  modification  to  the  plumbing  of  the  pump  and  accumulator  to 
include  an  electrically  controlled  solenoid  valve  operated  from  the 
Electronic  Control  Panel  (ECP); 

•  rewiring  the  pump  to  be  electrically  controlled  from  the  ECP; 

•  redesign  and  manufacture  of  the  waist  joint  (joint  0)  shaft  and 
coupling,  along  with  that  of  several  piston  end  plates;  and 

•  an  overhaul  of  the  complete  electronics. 

B.  PRESSURE  TRANSDUCERS 

Pressure  transducers  which  had  previously  been  installed  on  the 
NEPTUNE  II  arm  at  joints  1  and  2  (Figure  16)  allowed  for  data  collec- 
tion to  validate  the  two-dimensional  simulation  with  gravitational 
effects.  Initially,  the  pressure  transducers  had  to  be  calibrated. 
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Figure  15.    NEPTUNE  II  Robot 
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Joint  1 


Joint  2 


Figure  16.    Robot  Pressure  Transducers 
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A  3,000-pound  nitrogen  bottle  with  a  200  psi  regulator  was  used 
for  pressure  transducer  calibration.  This  was  connected  to  a  test  rig 
which  consisted  of  a  varlinear  pressure  adjustment  and  a  200-pound 
pressure  gage  which  had  been  calibrated  and  was  incremented  in  0.2- 
pound  increments  (Figure  17).  The  varlinear  allowed  the  pressure  at 
the  transducer  to  be  varied  in  fine  increments  from  zero  to  120  psi, 
which  was  the  stated  range  of  the  transducers.  The  electrical  connec- 
tions from  the  transducers  had  been  hard-wired  to  the  ECP  and  then 
to  a  digital  multimeter  (Fluke  8024  B);  the  meter  20-volt  scale  was 
used  to  read  the  voltage  output  of  the  transducers  (the  voltage  was 
read  to  the  second  decimal  place).  Each  Omega  Series  PX302  pressure 
transducer  had  two  complete  sets  of  data  from  0  to  120  psi;  the  aver- 
ages of  these  are  provided  in  Table  1 .  The  pressure  transducers  were 
very  linear  from  0  to  115  psi,  with  each  5  psi  producing  a  voltage  of 
0.5  volts  (Figure  18).  This  range  of  pressures  was  well  acceptable  for 
the  NEPTUNE  II,  which  operates  between  0  and  118  psi  [Ref.  12]. 
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PRESSURE  TRANSDUCER 


Figure  17.    Pressure  Transducer  Calibration  Set-Up 
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TABLE  1 
PRESSURE  TRANSDUCER  DATA 


Pressure 

PI 

P2 

P3 

P4 

(psi) 

volts 

volts 

volts 

volts 

000 

-0.010 

0.000 

0.015 

0.000 

005 

0.500 

0.510 

0.520 

0.500 

010 

0.995 

1.005 

1.025 

1.005 

015 

1.500 

1.500 

1.530 

1.500 

020 

1.985 

1.995 

2.020 

2.000 

025 

2.500 

2.500 

2.530 

2.500 

030 

3.000 

3.000 

3.030 

3.000 

035 

3.490 

3.500 

3.530 

3.490 

040 

3.990 

3.995 

4.030 

3.995 

045 

4.500 

4.500 

4.530 

4.500 

050 

4.995 

4.995 

5.025 

4.990 

055 

5.490 

5.500 

5.520 

5.490 

060 

5.995 

5.995 

6.020 

5.990 

065 

6.490 

6.500 

6.520 

6.490 

070 

6.990 

7.000 

7.020 

6.990 

075 

7.490 

7.500 

7.510 

7.500 

080 

7.990 

8.005 

8.010 

8.000 

085 

8.490 

8.510 

8.510 

8.500 

090 

8.990 

9.005 

9.000 

9.000 

095 

9.480 

9.500 

9.500 

9.500 

100 

9.990 

10.005 

10.000 

10.000 

105 

10.490 

10.510 

10.500 

10.500 

110 

10.990 

11.010 

11.000 

11.000 

115 

11.485 

11.490 

11.490 

11.500 

120 

11.580 

11.490 

11.990 

12.000 
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Figure  18.    Pressure  Transducer  Data 
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C.     MEASUREMENTS 
1.      Link  Lengths 

With  the  necessity  to  disassemble  the  NEPTUNE  II  for 
repairs,  the  link  length  measurements  were  physically  measured. 
These  measurements  are  provided  in  Table  2  to  the  nearest  1/8  inch. 


TABLE  2 

LINK   LENGTHS 

Length 
inch 

Length 
meter 

10.250 

0.2604 

16.375 

0.4159 

13.750 

0.3498 

2.     Link  Masses 

Also  during  the  disassembly  of  the  robot,  the  link  masses 
were  measured  using  a  scale  which  was  in  half-pound  increments.  The 
design  of  the  robot  allowed  for  a  good  approximation  of  the  two 
masses  per  link  as  used  in  the  simulation  (Figures  19  and  20).  This 
was  a  crude  measurement,  but  it  was  felt  that  this  was  sufficient  to  be 
used  for  the  simulation  and  that  the  time  necessary  to  completely  dis- 
assemble the  robot  in  order  to  be  able  to  obtain  closer  measurements 
was  not  justified.  The  masses  which  were  measured  are  provided  in 
Table  3. 
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Figure  19.    Robot  Link  Masses 
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Figure  20.    Simulation  Link  Masses 
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TABLE  3 

LINK 

MASSES 

Parameter 

Pounds 

Kilograms 

MASS(l.l)  est. 

5 

2.273 

MASS(1,2)  est. 

15 

6.818 

Ml   (measured) 

20 

9.091 

MASS(2,1)  est. 

1 

0.455 

MASS(2,2)  est. 

1 

0.455 

M2  (measured) 

2 

0.910 

MASS(3,1)  est. 

13 

5.909 

MASS(3,2)  est. 

13 

5.909 

M3  (measured) 

26 

11.818 

3.     Length  from  COG  of  Link  to  Center  of  Mass 

The  point  mass  centers  were  assumed  to  be  at  the  joints 
(Figure  20).  A  simple  calculation  of  the  moments  around  the  center  of 
gravity  was  performed;  this  provided  the  data  in  Table  4. 

TABLE  4 

LENGTH  FROM  LINK  CENTER  OF  GRAVITY 

TO  MASS  CENTER  OF  GRAVITY 

Parameter 


L(l,l) 
L(l,2) 
L(2,l) 
L(2,2) 
L(3,l) 
L(3,2) 


Length 
inch 

Length 
meter 

7.6875 

0.1953 

2.5625 

0.0651 

8.1875 

0.2080 

8.1875 

0.2080 

6.8750 

0.1746 

6.8750 

0.1746 

41 


D.  DATA  COLLECTION 

Position  data  and  pressure  data  were  taken  for  several  movements 
of  links  2  and  3  of  the  robot  from  the  electronic  control  panel  and 
recorded  on  a  four-channel  strip  chart  recorder.  An  example  of  the 
recorded  data  is  provided  (Figure  21). 

E.  DATA  REDUCTION 

The  data  taken  for  the  various  runs  was  reduced  using  the  follow- 
ing equation  to  obtain  joint  torques. 

TORQUE  =  A  PRESSURE  *  AREA(piston)  *  RADIUS(lever  arm)     (51) 

where  AREA(piston)  =  0.00312  square  meters  and  RADIUS(lever  arm) 
=  0.0597/2  meters. 

F.  VALIDATION 

The  delta  pressure  data  versus  time  was  entered  into  the  robot 
validation  program  as  a  time  function  and  the  torque  was  calculated  as 
a  function  of  time  from  this  data.  The  simulation  position  results 
agreed  with  the  actual  robot  position  in  the  general  trend  of  motion 
(Figure  22).  It  was  felt  that  the  inaccuracies  in  the  results  were  due  to 
both  the  simulation  round-off  errors  and  the  crude  methods  used  in 
the  collection  of  the  robot  link  masses  and  link  lengths,  particularly 
the  lengths  from  the  center  of  gravity  of  the  link  to  the  center  of 
gravity  of  the  point  masses.  The  scalloped  shape  of  the  simulated 
result  was  attributed  to  the  normalization  of  the  directional  cosines. 
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Figure  21.    Pressure  Recordings 
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Figure  22.   Robot/Simulation  Motion 
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VI.     RESULTS  AND  RECOMMENDATIONS 

1.  A  nonsingular  dynamic  simulation  of  a  rigid  revolute  three-link 
manipulator  has  been  further  developed.  It  includes: 

•  Gravity, 

•  Three-Dimensional  Motion, 

•  Comparatively  straightforward  dynamics. 

2.  Accuracy  and  nonsingularity  has  been  validated  with  a  double 
pendulum. 

3.  Three-dimensional  nonsingularity  has  been  validated. 

4.  Procedures  for  actual  robot  data  collection  have  been  developed. 

5.  The  following  recommendations  are  provided: 

•  Simulate  actual  three-dimensional  robot  performance  and 
compare  predicted  to  measured  variables. 

•  Enhance  the  computer  program  code  to  enable  it  to  be  more 
interactive.  That  is,  let  the  user  specify  the  constraints  during 
execution. 

•  Use  this  simulation  to  develop  advanced  controllers  for  rigid 
robot  linkages. 
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APPENDIX  A 
DIRECT  DYNAMIC  SIMULATION  PROGRAM 


TITLE  DIRECT  DYNAMICS   PROBLEM 

*  THIS    IS  A  PROGRAM  THAT  WILL  SOLVE  THE  DIRECT  DYNAMICS   PROBLEM  FOR  A  * 

*  3   LINK  RIGID  BODY  MANIPULATOR  WITHOUT  THE  USE   OF  ANY  TRANSFORMATION  * 

*  MATRICES.      THE   ORIGINAL  PROGRAM  WAS  WRITTEN  BY  LT  ATINOK  AND  HAS  * 

*  HAS   BEEN  GREATLY  MODIFIED  TO   INCLUDE   3   D  MOTION  AND  GRAVITATIONAL  * 
»  EFFECTS   BY               LT     R.    M.       VERBOS        USN                 SEPTEMBER   1988  * 

T»  7i  **  #{  #s  #v  r*  *»  *»  *v  /T«s<v  #v  *r7»  *t  7t*v  *v  *»  *»  *v  <\  #t  *»  *\  *\  •  *  7s  **  if  **  *s  «i  §%  **  »«  #v*»  #»  «*  »»  #»7>  *»  #\  *C*C«*7*  *«  *»  #*  **  #*  «v  *»  n  **  **  «»  <»  **  *»  *v  *v  7%  tV  7s  TT#C  *v 

TERMINAL 

METHOD  RKSFX 

PRINT   . 10,DRCANX(1),DRCANY(1),DRCANZ(1),.  .. 

DRCANX(2),DRCANY(2),DRCANZ(2),DRCANX(3),DRCANY(3),DRCANZ(3),.  .. 

DRCSX( 1) ,DRCSY( 1) ,DRCSZ( 1) ,DRCSX(2) ,DRCSY(2) ,DRCSZ(2) ,DRCSX(3) ,.  .  . 

DRCSY(3),DRCSZ(3),. .. 

JX0,JY0,JZ0,JX1,JY1,JZ1,JX2,JY2,JZ2,.  .  . 

LCOGX(l),LCOGY(l),LCOGZ(l),LCOGX(2),LCOGY(2),LCOGZ(2),.  .  . 

LC0GX(3),LC0GY(3),LC0GZ(3),.  .  . 

T0X,T0Y,T0Z,T1X,T1Y,T1Z,T2X,T2Y,T2Z,.  .  . 

WDX(1),WDY(1),WDZ(1),WDX(2),WDY(2),WDZ(2),WDX(3),WDY(3),WDZ(3),.  .  . 

AX1,AY1,AZ1,AX2,AY2,AZ2,AX3,AY3,AZ3,.  .  . 

W1X,W1Y,W1Z,W2X,W2Y,W2Z,W3X,W3Y,W3Z,. . . 

FX0,FY0,FZ0,FX1,FY1,FZ1,FX2,FY2,FZ2,.  .  . 

IXXT( 1 ) , IYYTC 1 ) , I ZZT( 1 ) , IXXT( 2 ) , IYYT( 2 ) , IZZT( 2 ) , IXXT( 3) , IYYT( 3) 

IZZT( 3) , IXYT( 1) , IYZT( 1) , IXZTf 1) , IXYT( 2) , IYZT( 2) , IXZT( 2) , IXYT( 3) , .  .  . 

IYZT(3),IXZT(3),.  .  . 

LNCOG1 , LNCOG2 , LNCOG3 , THZANG , COSTHZ , SINTHZ , TIPX ,TIPY , TIPZ , .  .  . 

ANG12X,ANG12Y,ANG12Z,ANG23X,ANG23Y,ANG23Z,.  .  . 

T1CH,T2CH,IER 

CONTROL  FINTIM  =1.5,    DELT  =   .0005,    DELMAX  =   .1,    DELPRT  =   .10 

D  DIMENSION  MATA( 27 ,27) ,MASS( 3,2) ,L( 3 ,2) ,RX(3,2) ,RY(3,2) ,RZ( 3,2) 

D  DIMENSION   IXX( 3,2) , IXZ(3 ,2) , IXY( 3,2) , IYY( 3 ,2) , IYZ(3 ,2) ,IZZ( 3,2) 

D  DIMENSION   IMAT( 3 ,3,3) ,NIMAT( 3,3) ,TMAT(3,3) ,TMATTR(3,3) ,MATTMP(3,3) 

D  DIMENSION  LIMAT(3,3,3) 

FIXED        IER,I,J,M,K,P,N,IA,IDGT,A,I1 

ARRAY  MATB(27),ABMATB(27),LCOGX(3),LCOGY(3),LCOGZ(3) 

ARRAY  VECTA0<3),VECTB0(3),VECTA1(3),VECTB1(3),VECTA2(3),VECTB2(3) 

ARRAY  VECTA(3),VECTB(3) 

ARRAY  WDX(3),WDY(3),WDZ(3),W1(3),W2(3),W3(3) 

ARRAY  RBG1(3),RAG1(3),RBG2(3),RAG2(3),RBG3(3) 

ARRAY  WKAREA(850) 

ARRAY   IXXT(3),IYYT(3),IZZT(3),IXYT(3),IXZT(3),IYZT(3) 
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ARRAY  DRCANX(3),DRCANY(3),DRCANZ(3) 
ARRAY  DRCSX(3),DRCSY(3),DRCSZ(3) 


INITIAL 

INPUT   ***** 

* 

INPUT  PARAMETER  CONSTANTS 

55 

A  =  15. ODO 
P  =  0. ODO 
W  =  2  *  PI 
IDGT  =  6 

G  =  9.81D0 

N  =  27 

M  =  1 

IA  =  27 

IER  =  0 

LEVELQ  =  0 

LEVLDQ  =  0 

* 

INPUT  JOINT  LOCATIONS  IN  METERS 
JXO  =  0.  ODO 
JYO  =  0.  ODO 
JZO  =  O.ODO 
JXl  =  0.  ODO 
JYl  =  0. ODO 
JZl  =  0.  2D0 
JX2  =  0.  ODO 
JY2  =  0.416D0 
JZ2  =  0.2D0 

* 

INPUT  TORQUE  CONSTANTS 
TOX    =  0. ODO 
TOY    =  O.ODO 
TOZ    =  0. ODO 
TIX    =  O.ODO 
TIY    =  0. ODO 
TIZ    =  O.ODO 
T2X    =  O.ODO 
T2Y    =  0.  ODO 
T2Z    =  0.  ODO 
TGI    =  0.  ODO 
TG2    =  0.  ODO 
TIFNC  =  C.ODO 
T2FNC   =  0.  ODO 

* 

INPUT  DISTANCE  FROM  CENTER  OF  LINK  TO 

CENTER 

* 

FOR  EACH  LINK  ENDS 
1.(1,1)  =  0.05D0 
L(l,2)  =  0.  15D0 
L(2,l)  =  0.20D0 
L(2,2)  =  0. 216D0 
L(3,l)  =  0.225D0 
L(3,2)  =  0.226D0 

OF  MASS 


*  INPUT  THE  LINK  LENGTHS  OF  THE  ROBOT 
LINKL1  =  0. 20D0 
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LINKL2  =  0.416D0 
LINKL3  =  0.451D0 


* 

110 


INPUT  MASS  AT  LINK  ENDS  IN  KILOGRAMS 


120 


MASS(l.l)  = 

11.  ODO 

MASS(1,2)  = 

33.  ODO 

MASS(2,1)  = 

2.2D0 

MASS(2,2)  = 

2.  2D0 

MASS(3,1)  = 

28.6D0 

MASS(3,2)  = 

28.  6D0 

INPUT  LOCATION 

OF  LINK  CENTERS  OF  GRAVITY 

LCOGX(l)  = 

0.  ODO 

XI 

LCOGX(l) 

LCOGY(l)  = 

0.  ODO 

Yl 

LCOGY(l) 

LCOGZ(l)  = 

0.  10D0 

Zl 

LCOGZ(l) 

LCOGX(2)  = 

0.  ODO 

X2 

LC0GX(2) 

LCOGY(2)  = 

0.  208D0 

Y2 

LC0GY(2) 

LCOGZ(2)  = 

0.  20D0 

Z2 

LC0GZ(2) 

LC0GX(3)  = 

0.  ODO 

X3 

LC0GX(3) 

LCOGY(3)  = 

0.  6415D0 

Y3 

LC0GY(3) 

LCOGZ(3)  = 

0.  2D0 

Z3 

LC0GZ(3) 

INPUT  MASS  OF  EACH  LINK  IN  KG 

Ml  =  44.  ODC 

) 

M2  =  4.  4D0 

M3  =  57.2DC 

) 

INPUT  ACCELERATIONS  OF  JOINT  ZERO 

AXO  =  0. ODO 

AYO  =  0. ODO 

AZO  =  0. ODO 

INPUT  THE  INITIAL  DIRECTION  COSINES 

DRCSX(l)  = 

O.ODO 

DRCSY(l)  = 

0.  ODO 

DRCSZ(l)  = 

l.ODO 

DRCSX(2)  = 

0.  ODO 

DRCSY(2)  = 

l.ODO 

DRCSZ(2)  = 

0.  ODO 

DRCSX(3)  = 

O.ODO 

DRCSY(3)  = 

l.ODO 

DRCSZ(3)  = 

O.ODO 

INPUT  THE  INITIAL  DIRECTION  COSINE  ANGLES 
DRCANX(l)  =  90. ODO 
DRCANY(l)  =  90. ODO 
DRCANZ(l)  =   O.ODO 
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Mr* 


160 


DRCANX(2)  = 

90.  0D0 

DRCANY(2)  = 

0.  ODO 

DRCANZ(2)  = 

90.  ODO 

DRCANX(3)  = 

90.  ODO 

DRCANY(3)  = 

0.  ODO 

DRCANZ(3)  = 

90.  ODO 

INITIALIZE   *i 

r-y-y-j- 

OMEGA  AND  OMEGA 

DOT 

DO  170  I  =  1 

,3 

W1(I) 

=  O.ODO 

W2(I) 

=  0.  ODO 

W3(I) 

=  O.ODO 

WDX(I) 

=  0.  ODO 

WDY(I) 

=  O.ODO 

WDZ(I) 

=  O.ODO 

CONTINUE 

THZ  =  o.oro 

170 


*  INITIALIZE  MATRIX  A  AND  B  TO  ZERO 

DO  180  I  =  1,27 
DO  175  J  =  1,27 

MATA(I,J)  =  0. ODO 
175  CONTINUE 

MATB(I)  =  O.ODO 
180        CONTINUE 

*****    CALCULATIONS    ***** 

*  WEIGHTS  (NEWTONS) 
185        WG1  =  Ml-G 

WG2  =  M2*G 
WG3  =  M3*G 

*  COMPUTE  THE  LENGTH  FROM  THE  INBOARD  JOINT  TO  COG 

LNC0G1  =  DSQRT  (  LCOGX( 1)*LC0GX( 1)  +  LCOGY( 1)*LC0GY( 1)  +.  .  . 

LC0GZ(1)*LC0GZ(1)  ) 

LX2  =  LC0GX(2)  -  JX1 

LY2  =  LC0GY(2)  -  JY1 

LZ2  =  LC0GZ(2)  -  JZ1 

LNC0G2  =  DSQRT  (  LX2*LX2  +  LY2*LY2  +  LZ2*LZ2  ) 

LX3  =  LC0GX(3)  -  JX2 

LY3  =  LC0GY(3)  -  JY2 

LZ3  =  LC0GZ(3)  -  JZ2 

LNC0G3  =  DSQRT  (  LX3*LX3  +  LY3*LY3  +  LZ3*LZ3  ) 

*  COMPUTE  INITIAL  INERTIAS  BASED  ON  POINT  MASSES 

*  IN  GLOBAL  COORDINATES 
190        DO  225  I  =  1,3 

RX(I,1)  =  -L(I,1)  *  DRCSX(I) 

RX(I,2)  =  L(I,2)  *  DRCSX(I) 

RY(I,1)  =  -L(I,1)  *  DRCSY(I) 

RY(I,2)  =  L(I,2)  *  DRCSY(I) 

RZ(I,1)  =  -L(I,1)  *  DRCSZ(I) 

RZ(I,2)  =  L(I,2)  *  DRCSZ(I) 
200   IXX(I,1)  =  MASS(I,1)  *  ((RY(I,1)  *  RY(I,1))  +  (RZ(I.l)  *  RZ(I,1))) 
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205 


IXX(I,2) 

IXXT(I) 

IYY(I,1) 


IYY(I 


IYYT(I)      = 


IZZ(I 
IZZ(I 


IZZT(I)      = 


IXY(I 
IXY(I 


IXYT(I)      = 


IXZ(I 
IXZ(I 


IXZT(I)      = 


IYZ(I 
IYZ(I 


2)    = 


1) 
2) 


1) 
2) 


1) 
2) 


1) 
2) 


MASS(I 
IXX(I, 
MASS(I 
MASS (I 
IYY(I, 
MASS (I 
MASS(I 
IZZ(I, 
MASS(I 
MASS(I 
IXY(I, 
MASS(I 
MASS(I 
IXZ(I, 
MASS(I 
MASS(I 
IYZ(I, 

.LE. 

02 


225 


IYZT(I) 

IF  (IXXT(I) 

IXXT(I)   = 

ELSE 

IXXT(I)  =  IXXT(I) 

END  IF 

IF  (IYYT(I)  .LE. 

IYYT(I)   =  .02 

ELSE 

IYYT(I)   =  IYYT(I 

END  IF 

IF  (IZZT(I)  .LE. 

IZZT(I)   =  .02 

ELSE 

IZZT(I)   =  IZZT(I 

END  IF 

IMAT(I,1,]) 
IMAT(I,1,2) 
IMAT(I,1,3) 
IMAT(I,2,1) 
IMAT(I,2,2) 
IMAT( 1,2,3) 
IMAT(I,3,1) 
IMAT(I,3,2) 
IMAT(I,3,3) 
CONTINUE 


,2)  *  ((RY(I,2) 
1)  +  IXX(I,2) 
,1)  *  ((RX(I,1) 
,2)  *  ((RX(I,2) 
1)  +  IYY(I,2) 
,1)  *  ((RX(I,1) 
,2)  *  ((RX(I,2) 
1)  +  IZZ(I,2) 
,1)  *  RX(I,1) 
,2)  *  RX(I,2) 
1)  +  IXY(I,2) 
,1)  *  RZ(I,1) 
,2)  *  RZ(I,2) 
1)  +  IXZ(I,2) 
,1)  *  RY(I,1) 
,2)  *  RY(I,2) 
1)  +  IYZ(I,2) 
. 02)  THEN 


*  RYU, 2))  +  (RZ(I,2)  *  RZ(I,2))) 


.02)  THEN 


) 

.02)  THEN 


) 


IXXT(I) 

IXYT(I) 

IXZT(I) 

-IXYT(I) 

IYYT(I) 

IYZT(I) 

-IXZT(I) 

-IYZT(I) 

IZZT(I) 


*  RX(I,1))  + 

*  RX(I,2))  + 

*  RX(I,1))  + 

*  RX(I,2))  + 

*  RY(I,1) 

*  RY(I, 2) 


(RZ(I,1) 
(RZ(I,2) 

(RY(I,1) 
(RYU, 2) 


*  RZ(I,1))) 

*  RZ(I,2))) 

*  RY(I,1))) 

*  RY(I, 2))) 


* 


RX(I,l) 
RX(I,2) 

RZ(I,1) 
RZ(I,2) 


DUE  TO  LINK 

IXXT(l)  = 

IXYT(l)  = 

IXZT(l)  = 

IYYT(l)  = 

IYZT(l)  = 

IZZT(l)  = 


1  CONSTRAINTS 
IMAT( 1,1,1) 
IMAT( 1,1,2) 
IMAT( 1,1,3) 
IMAT( 1,2,2) 
IMAT( 1,2,3) 
IMAT( 1,3,3) 


LINK  1  INERTIAS  ARE  CONSTANT 


TRANSFORM  THE  INITIAL  INERTIAS  TO  LOCAL  COORDINATED 
DO  9  1=2,  3 

TMAT(2,1)  =  -DCOS  (  THZ  ) 
TMAT(2,2)  =  -DSIN  (  THZ  ) 
TMAT(2,3)  =  0.0D0 
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TMAT(3,1)  =  DRCSX(I) 
TMAT(3,2)  =  DRCSY(I) 
TiMAT(3,3)  =  DRCSZ(I) 


VECTA(l)  = 
VECTA(2)  = 
VECTA(3)  = 
VECTB(l)  = 
VECTB(2)  = 
VECTB(3)  = 
CALL  CPROD  ( 
TMAT(1,1)  = 
TMAT(1,2)  = 
TMAT(1,3)  = 
TMATTR(1,1) 
TMATTR(1,2) 
TMATTR(1,3) 
TMATTR(2,1) 
TMATTR(2,2) 
TMATTR(2,3) 
TMATTR(3,1) 
TMATTR(3,2) 
TMATTR(3,3) 


TMAT(2,1) 

TMAT(2,2) 

TMAT(2,3) 

TMAT(3,1) 

TMAT(3,2) 

TMAT(3,3) 

VECTA,VECTB,MI1,MJ1,MK1) 

Mil 

MJ1 

MK1 

=  TMAT(1,1) 

=  TMAT(2,1) 

=  TMAT(3,1) 

=  TMAT(1,2) 

=  TMAT(2,2) 

=  TMAT(3,2) 

=  TMAT(1,3) 

=  TMAT(2,3) 

=  TMAT(3,3) 


DO  5  II  =  1,3 
DO  5  J  =1,3 
TEMP  =  0.  ODO 
DO  1   K  =1,3 

TEMP  =  TMAT(I1,K)  *  IMAT(I,K,J)  +  TEMP 


1 

CONTINUE 

MATTMP(I1,J) 

=  TEMP 

5 

CONTINUE 

DO  8   11  = 
DO  8   J  = 
TEMP  =  0. 

1,3 
1,3 

ODO 

DO  7    K 

=  1, 

3 

TEMP  = 

MATTMP(I1,K) 

*  TMATTR(K,J) 

+  TEMP 

7 

CONTINUE 

LIMAT(I.I1, 

,J)  = 

■   TEMP 

8 

CONTINUE 

9 

CONTINUE 

DERIVATIVE 

NOSORT 

230        CALL  ERRSET  (208,256,-1,1,1) 

CALL  UERSET( LE VELQ , LEVLDQ ) 
*     INITIALIZE  MATRIX  A  AND  B  TO  ZERO 
DO  240  I  =  1,27 
DO  235  J  =  1,27 

MATA(I.J)  =  O.ODO 
235  CONTINUE 

MATB(I)  =  O.ODO 
240        CONTINUE 


it 


INPUT  JOINT  EQUATIONS 
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JOINT  ZERO  EQUATIONS 
Wl  X  (Wl  X  RB/G1) 

RBGl(l)  =  JXO  -  LCOGX(l) 

RBG1(2)  =  JYO  -  LCOGY(l) 

RBG1(3)  =  JZO  -  LCOGZ(l) 

VECTA(l)  =  Wl(l) 

VECTA(2)  =  Wl(2) 

VECTA(3)  =  Wl(3) 
CALL  CPR0D(VECTA0,RBG1, MICO, MJCO, MKCO) 

VECTBO(l)  =  MICO 

VECTB0(2)  =  MJCO 

VECTB0(3)  =  MKCO 
CALL  CPROD( VECTAO , VECTBO , MICO , MJCO , MKCO ) 

*  INPUT  JOINT  EQUATIONS 

*  JOINT  ZERO  EQUATIONS 

*  Wl  X  (Wl  X  RB/G1) 

RBG1(1)   =  JXO  -  LCOGX(l) 

RBG1(2)   =  JYO  -  LCOGY(l) 

RBG1(3)   =  JZO  -  LCOGZ(l) 

VECTA(l)  =  Wl(l) 

VECTA(2)  =  Wl(2) 

VECTA(3)  =  Wl(3) 
CALL  CPROD(VECTA,RBGl, MICO, MJCO, MKCO) 

VECTB(l)  =  MICO 

VECTB(2)  =  MJCO 

VECTB(3)  =  MKCO 
CALL  CPROD( VECTA , VECTB , MICO , MJCO , MKCO ) 
Wl  X  (Wl  X  RA/G1) 

RAG1(1)  =  JX1  -  LCOGX(l) 

RAG1(2)  =  JY1  -  LCOGY(l) 

RAG1(3)  =  JZ1  -  LCOGZ(l) 

VECTA(l)  =  Wl(l) 

VECTA(2)  =  Wl(2) 

VECTA(3)  =  Wl(3) 
CALL  CPROD  (VECTA, RAG1 ,MIC1 ,MJC1 ,MKC1) 

VECTB(l)  =  MIC1 

VECTB(2)  =  MJC1 

VECTB(3)  =  MKC1 
CALL  CPROD  (VECTA, VECTB, MIC 1,MJC1,MKC1) 

*  W2  X  (W2  X  RB/G2) 

RBG2(1)  =  JX1  -  LCOGX(2) 

RBG2(2)  =  JY1  -  LCOGY(2) 

RBG2(3)  =  JZ1  -  LCOGZ(2) 

VECTA(l)  =  W2(l) 

VECTA(2)  =  W2(2) 

VECTA(3)  =  W2(3) 
CALL  CPROD  ( VECTA, RBG2,MIC2,MJC2,MKC2) 

VECTB(l)  =  MIC2 

VECTBT2)  =  MJC2 

VECTB(3)  =  MKC2 
CALL  CPROD  (VECTA, VECTB, MIC2,MJC2,MKC2) 

*  W2  X  (W2  X  RA/G2) 

RAG2(1)  =  JX2  -  LCOGX(2) 
RAG2(2)  =  JY2  -  LC0GY(2) 
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RAG2(3)  =  JZ2  -  LC0GZ(2) 

VECTA(l)  =  W2(l) 

VECTA(2)  =  W2(2) 

VECTA(3)  =  W2(3) 
CALL  CPROD  (VECTA,RAG2,MIC3,MJC3,MKC3) 

VECTB(l)  =  MIC3 

VECTB(2)  =  MJC3 

VECTB(3)  =  MKC3 
CALL  CPROD(VECTA,VECTB,MIC3,MJC3,MKC3) 
*      W3  X  (W3  X  RB/G3) 

RBG3(1)  =  JX2  -  LCOGX(3) 

RBG3(2)  =  JY2  -  LCOGY(3) 

RBG3(3)  =  JZ2  -  LCOGZ(3) 

VECTA(l)  =  W3(l) 

VECTA(2)  =  V3(2) 

VECTA(3)  =  W3(3) 
CALL  CPROD  (VECTA,RBG3,MIC4,MJC4,MKC4) 

VECTB(l)  =  MIC4 

VECTB(2)  =  MJC4 

VECTB(3)  =  MKC4 
CALL  CPROD  (VECTA,VECTB,MIC4,MJC4,MKC4) 


* 


INERTIA  TRANSFORMATION 


DO  390  I  =  2,  3 


TMAT(2,1) 
TMAT(2,2) 
TMAT(2,3) 
TMAT(3,1) 
TMAT( 3,2) 
TMAT(3,3) 


-DCOS  ( 
-DSIN  ( 
0.  0D0 
DRCSX(I) 
DRCSY(I) 
DRCSZ(I) 


THZ 
THZ 


VECTA(l) 

VECTA(2) 

VECTA(3) 

VECTB(l) 

VECTB(2) 

VECTB(3) 

CALL  CPROD 

TMAT(1,1)  = 

TMAT(1,2)  = 

TMAT(1,3)  = 

TMATTR(1,1) 

TMATTR(1,2) 

TMATTR(1,3) 

TMATTR(2J1) 

TMATTR(2,2) 

TMATTR(2,3) 

TMATTR(3,1) 

TMATTR(3,2) 

TMATTR(3,3) 


TMAT(2,1) 
TMAT(2,2) 
TMAT(2,3) 
TMAT(3,1) 
TMAT(3,2) 
TMAT( 3,3) 
(VECTA,VECTB,MI1,MJ1,MK1) 
Mil 
MJ1 
MK1 

=  TMAT(1,1) 
=  TMAT(2,1) 
=  TMAT(3,1) 
=  TMAT(1,2) 
=  TMAT(2,2) 
=  TMAT(3,2) 
=  TMAT(1,3) 
=  TMAT(2,3) 
=  TMAT(3,3) 


DO  375  11  =  1,3 
DO  375  J  =  1,3 
TEMP  =  0. ODO 
DO  370  K  =  1,3 
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370 
375 


TEMP  =  TMATTR(I1,K)  *  LIMAT(I,K,J)  +  TEMP 
CONTINUE 
MATTMP(I1,J)  =  TEMP 
CONTINUE 


DO  380  11  =  1,3 

DO  380  J  =1,3 

TEMP  =  0.  0D0 

DO  378  K  =  1,3 

TEMP  =  MATTMP(I1,K)  *  TMAT(K,J)  +  TEMP 

378 

CONTINUE 

NIMAT(I1,J)  =  TEMP 

380 

CONTINUE 

IXXT(I)  =  NIMAT(1,1) 

IXYT(I)  =  NIMAT(1,2) 

IXZT(I)  =  NIMAT(1,3) 

IYYT(I)  =  NIMAT(2,2) 

IYZT(I)  =  NIMAT(2,3) 

IZZT(I)  =  NIMAT(3,3) 

390 

CONTINUE 

•sWo'oWc 

ENTER  CONSTANTS  INTO  MATRIX  A  AND  B  ***** 

** 

LINK  ONE 

-,V 

SUM  OF  FORCES 

* 

X  DIRECTION 

395 

MATA(1,1)   =   1.0D0 

MATA(1,4)   =  Ml 

MATA(1,10)  =  -1.  0D0 

MATB(l)     =   0.0D0 

* 

Y  DIRECTION 

MATA(2,2)   =   1.  0D0 

MATA(2,5)   =  Ml 

MATA(2,11)  =  -1. 0D0 

MATB(2)     =   0.  0D0 

* 

Z  DIRECTION 

MATA(3,3)   =   1.0D0 

MATA(3,6)   =  Ml 

MATA(3,12)  =  -1.  0D0 

MATB(3)    =  -WG1 

* 

EQUATIONS  AT  JOINT  ZERO 

* 

X  DIRECTION 

MATA(4,4)  =   1.  0D0 

MATA(4,8)  =  RBG1(3) 

MATA(4,9)  =  -RBG1(2) 

MATB(4)    =  AXO  -  MICO 

* 

Y  DIRECTION 

MATA(5,5)  =   1.  0D0 

MATA(5,7)  =  -RBG1(3) 

MATA(5,9)  =  RBGl(l) 

MATB(5)   =  AYO  -  MJCO 

* 

Z  DIRECTION 

MATA(6,6)  =   1.  0D0 

MATA(6,7)  =  RBG1(2) 

MATA(6,8)  =  -RBGl(l) 
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MATB(6) 

AZO  -  MKCO 

* 

SUM  OF  MOMENTS 

EQUATIONS 

* 

X  DIRECTION 

MATA(7,2)   = 

RBG1(3) 

MATA(7,3)   = 

-RBG1(2) 

MATA(7,7)   = 

-IXXT(l) 

MATA(7,8)   = 

IXYT(l) 

MATA(7,9)   = 

IXZT(l) 

MATA(7,11)  = 

-RAG1(3) 

MATA(7,12)  = 

RAG1(2) 

MATB(7) 

T1X  -  TOX 

* 

Y  DIRECTION 

MATA(8,1)   = 

-RBG1(3) 

MATA(8,3)   = 

RBGl(l) 

MATA(8,7)   = 

IXYT(l) 

MATA(8,8)   = 

-IYYT(l) 

MATA(8,9)   = 

IYZT(l) 

MATA(8,10)  = 

RAG1(3) 

MATA(8,12)  = 

-RAG1(1) 

MATB(8) 

T1Y  -  TOY 

* 

Z  DIRECTION 

MATA(9,1)   = 

RBG1(2) 

MATA(9,2)   = 

-RBG1(1) 

MATA(9,7)   = 

IXZT(l) 

MATA(9,8)   = 

IYZT(l) 

MATA(9,9)   = 

-IZZT(l) 

MATA(9,10)  = 

-RAG1(2) 

MATA(9,11)  = 

RAG1(1) 

MATB(9) 

T1Z  -  TOZ 

** 

LINK  TWO 

* 

SUM  OF  FORCES 

rt 

X  DIRECTION 

460 

MATA(10,10)  = 

1.0D0 

MATA(10,13)  = 

M2 

MATA(10,19)  = 

-1.  0D0 

MATB(IO) 

0.  ODO 

* 

Y  DIRECTION 

MATA(11,11)  = 

l.ODO 

MATA(11,14)  = 

M2 

MATA( 11,20)  = 

-1.  ODO 

MATB(ll) 

0.  ODO 

* 

Z  DIRECTION 

MATA(12,12)  = 

1.  ODO 

MATA(12,15)  = 

M2 

MATA( 12,21)  = 

-1.  ODO 

MATB(12) 

-WG2 

* 

EQUATIONS  AT  JOINT  ONE 

* 

X  DIRECTION 

MATA(13,4)   = 

-l.ODO 

MATA(13,8)   = 

-RAG1(3) 

MATA(13,9)   = 

RAG1(2) 

MATA(13,13)  = 

1.  ODO 

MATA(13,17)  = 

RBG2(3) 

MATA(13,18)  = 

-RBG2(2) 

MATB(13) 

MIC1  -  MIC2 

A 

Y  DIRECTION 
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MATA(14,5) 

= 

-1.  0D0 

MATA(14,7) 

= 

RAG1(3) 

MATA(14,9) 

= 

-RAGl(l) 

MATA(14,14) 

= 

1.  0D0 

MATA(14,16) 

= 

-RBG2(3) 

MATA(14,18) 

= 

RBG2(1) 

MATB(14) 

= 

MJC1  -  MJC2 

*      Z  DIRECTION 

MATA(15,6) 

= 

-1.  0D0 

MATA(15,7) 

= 

-RAG1(2) 

MATA(15,8) 

= 

RAGl(l) 

MATA(15,15) 

= 

1.  0D0 

MATA( 15,16) 

= 

RBG2(2) 

MATA(15,17) 

ss 

-RBG2(1) 

MATB(15) 

= 

MKC1  -  MKC2 

*      SUM  OF  MOMENTS  EQUATIONS 

*      X  DIRECTION 

MATA(16,11) 

= 

RBG2(3) 

MATA(16,12) 

= 

-RBG2(2) 

MATA(16,16) 

= 

-IXXT(2) 

MATA(16,17) 

ss 

IXYT(2) 

MATA(16,18) 

ss 

IXZT(2) 

MATA( 16,20) 

ss 

-RAG2(3) 

MATA( 16,21) 

= 

RAG2(2) 

MATB(16) 

ss 

T2X  -  T1X 

*      Y  DIRECTION 

MATA(17,10) 

= 

-RBG2(3) 

MATA(17,12) 

= 

RBG2(1) 

MATA(17,16) 

= 

IXYT(2) 

MATA(17,17) 

= 

-IYYT(2) 

MATA(17,18) 

= 

IYZT(2) 

MATA( 17,19) 

ss 

RAG2(3) 

MATA( 17,21) 

= 

-RAG2(1) 

MATB(17) 

= 

T2Y  -  T1Y 

*      Z  DIRECTION 

MATA(18,10) 

= 

RBG2(2) 

MATA(18,11) 

= 

-RBG2(1) 

MATA(18,16) 

= 

IXZT(2) 

MATA(18,17) 

= 

IYZT(2) 

MATA(18,18) 

ss 

-IZZT(2) 

MATA(18,19) 

= 

-RAG2(2) 

MATA( 18,20) 

= 

RAG2(1) 

MATB(18) 

— 

T2Z  -  T1Z 

**     LINK  THREE 

*      SUM  OF  FORCES 

*      X  DIRECTION 

530      MATA(19,19) 

= 

1.0D0 

MATA( 19,22) 

ss 

M3 

MATB(19) 

= 

0.0D0 

*      Y  DIRECTION 

MATA(20,20) 

= 

1.0D0 

MATA(20,23) 

= 

M3 

MATB(20) 

= 

0.0D0 

*      Z  DIRECTION 

MATA(21,21) 

= 

1.  0D0 

MATA(21,24) 

ss 

M3 
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* 


* 


MATB(21)  =  -W 
EQUATIONS  AT  JO 
X  DIRECTION 
MATA(22,13)  = 
MATA(22,17)  = 
MATA(22,18)  = 
MATA(22,22)  = 
MATA( 22,26)  = 
MATA(22,27)  = 
MATB(22) 

Y  DIRECTION 
MATA(23,14)  = 
MATA(23,16)  = 
MATA(23,18)  = 
MATA(23,23)  = 
MATA(23,25)  = 
MATA(23,27)  = 
MATB(23) 

Z  DIRECTION 
MATA(24,15)  = 
MATA(24,16)  = 
MATA(24,17  :  = 
MATA(24,24)  = 
MATA( 24,25)  = 
MATA(24,26)  = 
MATB(24) 

SUM  OF  MOMENTS 

X  DIRECTION 
MATA(25,20) 
MATA(25,21) 
MATA(25,25) 
MATA(25,26) 
MATA(25,27) 
MATB(25) 

Y  DIRECTION 
MATA(26,19) 
MATA(26,21) 
MATA(26,25) 
MATA(26,26) 
MATA(26,27) 
MATB(26) 

Z  DIRECTION 
MATA(27,19) 
MATA(27,20) 
MATA(27,25) 
MATA(27,26) 
MATA(27,27) 
MATB(27) 


G3 

INT  TWO 

-1.  0D0 

-RAG2(3) 

RAG2(2) 

1.  ODO 

RBG3(3) 
-RBG3(2) 

MIC3  -  MIC4 

-1.  ODO 

RAG2(3) 
-RAG2(1) 

1.  ODO 
-RBG3(3) 

RBG3(1) 

MJC3  -  MJC4 

-1.  ODO 
-RAG2(2) 

RAG2(1) 

1.  ODO 

RBG3(2) 
-RBG3(1) 

MKC3  -  MKC4 
EQUATIONS 

RBG3(3) 
-RBG3(2) 
-IXXT(3) 

IXYT(3) 

IXZT(3) 
-T2X 

-RBG3(3) 

RBG3(1) 

IXYT(3) 
-IYYT(3) 

>  IYZT(3) 
:  -T2Y 

RBG3(2) 

>  -RBG3(1) 
'  IXZT(3) 
!  IYZT(3) 
i  -IZZT(3) 
i  -T2Z 


590 


595 


FIRST  LINK  IS  CONSTRAINED  TO  ROTATE  IN  Z  DIRECTION 
MODIFIED  BY  RM  VERBOS 
DO  600  I  =  4,8 
DO  595  J  =  1,27 
MATA(I,J)  =  0. ODO 
MATA(J,I)  =  0.  ODO 
CONTINUE 


ickiric/e 
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MATB(I)  =  0. 

0D0 

600 

CONTINUE 

MATA(4,4)  = 

l.ODO 

MATA(5,5)  = 

1.  ODO 

MATA(6,6)  = 

1.  ODO 

MATA(7,7)  = 

1.  ODO 

MATA(8,8)  = 

l.ODO 

605 

DO  610  J  =  1,27 

MATA(18,J)  = 

0.  ODO 

MATA(27,J)  = 

0.  ODO 

610 

CONTINUE 

MATA(9,9)  =  -(IZZT(1)+IZZT(2)+IZZT(3)) 

MATA(18,9)  =  -1.  ODO 

MATA(18,18)  =   l.ODO 

MATB(18)  =   O.ODO 

MATA(27,9)  =  -1. ODO 

MATA(27,27)  =   l.ODO 

MATB(27)  =  O.ODO 

*      CALL  EQUATION  SOLVER  PROGRAM  FROM  IMSL 

620        CALL  LEQT2F(MATA,M,N,IA,MATB,IDGT,WKAREA,IER) 

IF  (IER.  EQ.  0)  THEN 

GOTO  640 

ELSE 

WRITE  (*,624)  IER 
624        FORMAT  (15) 

DO  635   1=1,  27 

WRITE  (*,627)  I 
627        FORMAT  (17) 

DO  631   J  =  1,  27,  3 

WRITE  (*,630)  J,MATA(I,J),J+l,MATA(I,J+l),J+2,MATA(I,J+2) 

630  FORMAT  ( 15 ,F13. 5 , 15 ,F13. 5 , 15 ,F13. 5) 

631  CONTINUE 

WRITE  (-,633)  I,MATB(I) 
633        FORMAT  (I3.F15.5) 
635        CONTINUE 

CALL  ENDJOB 

END  IF 

FIND  LCOGX,LCOGY,LCOGZ,THETA  VALUE S,WX,WY,WZ 


* 

JOINT  ZERO 

640 

FXO  =  MATB(l) 

FYO  =  MATB(2) 

FZO  =  MATB(3) 

* 

LINK  ONE 

* 

SINCE  LINK1  IS  CONSTRAIN  TO  ROTATE  AROUND  THE  Z  ONLY 

AX1  =  0. ODO 

AY1  =  0.  ODO 

AZ1  =  O.ODO 

*660 

AX1       =  MATB(4) 

* 

VELX1     =  INTGRL(0. ,AX1) 

* 

LC0GX1    =  INTGRL(X1,VELX1) 

* 

LCOGX(l)   =  LC0GX1 
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* 

AY1 

=  MATB(5) 

* 

VELY1 

=  INTGRL(0.  ,AY1) 

* 

LC0GY1 

=  INTGRL(Y1,VELY1) 

* 

LCOGY(l) 

=  LCOGY1 

* 

AZ1 

=  MATB(6) 

* 

VELZ1 

=  INTGRL(0.  ,AZ1) 

* 

LCOGZ1 

=  INTGRL(Z1,VELZ1) 

* 

LCOGZ(l) 

=  LCOGZ1 

WD  IX 

=  MATB(7) 

W1X 

=  INTGRL(0.  ,WD1X) 

WDX(l) 

=  WD1X 

Wl(l) 

=  W1X 

WD1Y 

=  MATB(8) 

W1Y 

=  INTGRL(0. ,WD1Y) 

WDY(l) 

=  WD1Y 

Wl(2) 

=  W1Y 

WD1Z 

=  MATB(9) 

W1Z 

=  INTGRL(0.  ,WD1Z) 

VDZ(l) 

=  WD1Z 

Wl(3) 

=  W1Z 

*** 

ADDED  BY  '. 

R.  M.  VERBOS 

685 

THZ 

=  INTGRL(0.  ,W1Z) 

THZANG 

=  THZ  *  RADEG 

COSTHZ 

=  DCOS(THZ) 

SINTHZ 

=  DSIN(THZ) 

ft 

ft 
ft 
* 

*700 
ft 

ft 

* 

ft 

* 

ft 

*720 


IF  THE  1ST  LINK  IS  CONSTRAIN  TO  ROTATE  IN  THE  Z  DIRECTION  ONLY 
THE  DIRECTION  COSINE  AND  DIRECTION  COSINE  ANGLES  ARE  CONSTANT 
AND  DO  NOT  NEED  TO  BE  CALCULATED 

CALC  DIRECTIONAL  COSINES  FOR  LINK  ONE 

DRCSX(l)   =  LCOGX1  /  LNCOG1 

DRCSY(l)   =  LCOGY1  /  LNCOG1 

DRCSZ(l)   =  LCOGZ1  /  LNCOG1 
AMP  =  DSQRT(DRCSX(1)*DRCSX(1)+DRCSY(1)*DRCSY(1)+. . . 

DRCSZ(1)*DRCSZ(1)) 
DRCSX(l)  =  DRCSX(1)/AMP 
DRCSY(l)  =  DRCSY(1)/AMP 
DRCSZ(l)  =  DRCSZ(1)/AMP 
DRCANX(l)  =  DACOS(DRCSX(l))  *  RADEG 
DRCANY(l)  =  DACOS(DRCSY(l))  *  RADEG 
DRCANZ(l)  =  DACOS(DRCSZ(l))  *  RADEG 


ft 

*ft 

JOINT  LOCATION 

*730 

JX1  =  LINKL1  *  DRCSX(l) 

ft 

JY1  =  LINKL1  *  DRCSY(l) 

ft 

JZ1   =  LINKL1  *  DRCSZ(l) 

J. 

JOINT  ONE 

735 

FX1  =  MATB(IO) 

FY1  =  MATB(ll) 

FZ1  =  MATB(12) 

ftft 

LINK  TWO 

740 

AX2       =  MATB(13) 

VELX2     =  INTGRL(0. ,AX2) 
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LC0GX2 

= 

INTGRL(X2 

,VELX2) 

LC0GX(2) 

= 

LC0GX2 

AY2 

= 

MATB(14) 

VELY2 

= 

INTGRL(0. 

,AY2) 

LC0GY2 

ss 

INTGRL(Y2 

,VELY2) 

LC0GY(2) 

= 

LC0GY2 

AZ2 

=: 

MATB(15) 

VELZ2 

= 

INTGRL(0. 

,AZ2) 

LC0GZ2 

=: 

INTGRL(Z2 

,VELZ2) 

LC0GZ(2) 

= 

LC0GZ2 

WD2X 

= 

MATB(16) 

W2X 

=: 

INTGRL(0. 

,WD2X) 

VDX(2) 

= 

WD2X 

W2(l) 

= 

W2X 

WD2Y 

= 

MATB(17) 

W2Y 

= 

INTGRL(0. 

,WD2Y) 

WDY(2) 

= 

WD2Y 

W2(2) 

= 

W2Y 

WD2Z 

= 

MATB(18) 

W2Z 

=r 

INTGRL(0. 

,WD2Z) 

WDZ(2) 

= 

WD2Z 

W2(3) 

s: 

V2Z 

*      GET  THE  DIRECTION  COSINES  FOR  THE  LINK  TWO 
DRCSX(2)   =  (LCOGX2  -  JX1)  /  LNCOG2 
DRCSY(2)   =  (LCOGY2  -  JY1)  /  LNCOG2 
DRCSZ(2)   =  (LCOGZ2  -  JZ1)  /  LNCOG2 

790      AMP  =  DSQRT(DRCSX(2)*DRCSX(2)+DRCSY(2)*DRCSY(2)+. 

DRCSZ(2)*DRCSZ(2)) 
DRCSX(2)  =  DRCSX(2)/AMP 
DRCSY(2)  =  DRCSY(2)/AMP 
DRCSZ(2)  =  DRCSZ(2)/AMP 
DRCANX(2)  =  DACOS(DRCSX(2))  *   RADEG 
DRCANY(2)  =  DACOS(DRCSY(2))  *  RADEG 
DRCANZ(2)  =  DAC0S(DRCSZ(2))  *  RADEG 

JOINT  LOCATION 
800      JX2  =  JX1  +  LINKL2  *   DRCSX(2) 
JY2  =  JY1  +  LINKL2  *  DRCSY(2) 
JZ2  =  JZ1  +  LINKL2  *  DRCSZ(2) 


*  JOINT  TWO 

805      FX2  =  MATB( 

FY2  =  MATB( 

FZ2  =  MATB( 
**     LINK  THREE 
812      AX3 

VELX3 

LCOGX3 

LCOGX(3)   = 

AY3 

VELY3 

LCOGY3 

LCOGY(3)   * 

AZ3 

VELZ3 


19) 
20) 
21) 

MATB(22) 

INTGRL(0.  ,AX3) 

INTGRL(X3,VELX3) 

LCOGX3 

MATB(23) 

INTGRL(0.  ,AY3) 

INTGRL(Y3,VELY3) 

LCOGY3 

MATB(24) 

INTGRL(0.  ,AZ3) 
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LC0GZ3 

=  INTGRL(Z3 

,VELZ3) 

LC0GZ(3) 

=  LC0GZ3 

WD3X 

=  MATB(25) 

W3X 

=  INTGRL(0. 

,WD3X) 

WDX(3) 

=  WD3X 

W3(l) 

=  W3X 

WD3Y 

=  MATB(26) 

W3Y 

=  INTGRL(0. 

,WD3Y) 

WDY(3) 

=  WD3Y 

W3(2) 

=  W3Y 

WD3Z 

=  MATB(27) 

W3Z 

=  INTGRL(0. 

,WD3Z) 

WDZ(3) 

=  WD3Z 

W3(3) 

=  W3Z 

*  CALC  DIRECTIONAL  COSINES  FOR  LINK  THREE 


845 


865 


DRCSX(3)  =  (LCOGX3  -  JX2)  /  LNCOG3 
DRCSY(3)  =  (LCOGY3  -  JY2)  /  LNCOG3 
DRCSZ(3)  =  (LCOGZ3  -  JZ2)  /  LNCOG3 
AMP  =  DSQRT(DRCSX(3)*DRCSX(3)+DRCSY(3)*DRCSY(3)+. 

DRCSZ(3)*DRCSZ(3)) 
DRCSX(3)  =  DRCSX(3)/AMP 
DRCSY(3)  =  DRCSY(3)/AMP 
DRCSZ(3)  =  DRCSZ(3)/AMP 
DRCANX(3)  =  DAC0S(DRCSX(3))  *  RADEG 
DRCANY(3)  =  DACOS(DRCSY(3))  *  RADEG 
DRCANZ(3)  =  DACOS(DRCSZ(3))  *  RADEG 


*   TIP  LOCATION 

875  TIPX  =  JX2  +  LINKL3  *  DRCSX(3) 
TIPY  =  JY2  +  LINKL3  *  DRCSY(3) 
TIPZ  =  JZ2  +  LINKL3  *  DRCSZ(3) 


880 


FIND  THE  ANGLE  BETWEEN  THE  LIMKS 
ANG23X  =  DRCANX(2)  -  DRCANX(3) 
ANG23Y  =  DRCANY(2)  -  DRCANY(3) 
ANG23Z  =  DRCANZ(2)  -  DRCANZ(3) 


ANG12X  =  DRCANX(l)  -  DRCANX(2) 
ANG12Y  =  DRCANY(l)  -  DRCANY(2) 
ANG12Z  =  DRCANZ(l)  -  DRCANZ(2) 


DYNAMIC 

NOSORT 

*****   INPUT  TORQUE  AT  JOINTS 

*  CALCULATE  THE  MAGNITUDE  OF  THE  LENGTH  OF  THE  PROJECTION  OF 

*  LINK  2  AND  3  IN  THE  X  Y  PLANE  WHICH  IS  THE  MOMENT  ARM  FOR 

*  THE  CALCULATION  OF  THE  GRAVITATIONAL  TORQUE 


900 


MARM2A  =  DSQRT  (  LCOGX2  *  LC0GX2  +  LCOGY2  *  LCOGY2  ) 
MARM2B  =  DSQRT  (  LCOGX3  *  LCOGX3  +  LCOGY3  *  LCOGY3  ) 
MARM3   =  DSQRT  (  (LCOGX3  -  JX2)  *  (LCOGX3  -  JX2)  +.  . 

(LCOGY3  -  JY2)  *  (LCOGY3  -  JY2)  ) 


CALCULATE  GRAVITATIONAL  TORQUES 
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no 

T1FNC 

* 

T2FNC 

* 

TOZ 

* 

TOZ 

* 

TOZ 

TGI  =  MARM2A  *  WG2  +  MARM2B  *  WG3 
TG2  =  MARM3  *  WG3 

*  INPUT  TORQUES  OVERTOP  OF  GRAVITY 

2*A  -  A  *  DCOS  (  W  *  TIME  ) 
-3  *  A  *  DSIN  (W*TIME) 
A  *  DSIN  (  W  *  TIME  ) 
A  -  A  *  DSIN  (  W  *  TIME  ) 
50.0  -  50.0  *  TIME 

*  CALCULATE  TOTAL  TORQUE  ON  JOINT  1  AND  2 

Tl  =  TGI  +  T1FNC 
T2  =  TG2  +  T2FNC 

*  CALCULATE  X  AND  Y  COMPONENT  OF  TORQUE  ON  JOINT  1  AND  2 

T1X  =  Tl  *  DCOS  (THZ) 

T1Y  =  Tl  *  DSIN  (THZ) 

T2X  =  T2  *  COS  (THZ) 

T2Y  =  T2  *  SIN  (THZ) 

*  TORQUE  CHECK 

T1CH  =  DSQRT  (  T1X  *  T1X  +  T1Y  *  T1Y  ) 
T2CH  =  DSQRT  (  T2X  *  T2X  +  T2Y  *  T2Y  ) 

END 
STOP 

FORTRAN 

*  SUBROUTINE  TO  COMPUTE  THE  CROSS  PRODUCT  OF  TWO  VECTORS 

SUBROUTINE  CPROD( VECTA , VECTB , MI , MJ , MK) 
940  IMPLICIT  REAL- 8  (A-Z) 

DIMENSION  VECTA(3),VECTB(3) 

MI  =  VECTA(2)  *  VECTB(3)  -  VECTA(3)  *  VECTB(2) 
MJ  =  VECTA(3)  *  VECTB(l)  -  VECTA(l)  *  VECTB(3) 
MK  =  VECTA(l)  *  VECTB(2)  -  VECTA(2)  *  VECTB(l) 
RETURN 

END 
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APPENDIX  B 


DOUBLE  PENDULUM  VALIDATION  PROGRAM 


TITLE  DOUBLE  PENDULUM  VALIDATION  PROGRAM 


************************************************************************* 

*  THIS    IS  A  PROGRAM  THAT  WILL  SOLVE  THE  DIRECT  DYNAMICS   PROBLEM  FOR  A  * 

*  2   LINK  RIGID  BODY  MANIPULATOR  WITHOUT  THE  USE  OF  THE   STANDARD  TRAN-  * 

*  FORMATION  MATRICES  AND  VALIDATE  THE  MOTION  BY  THE  USE   OF  THE  DOUBLE  * 

*  PENDULUM  PROBLEM.  * 

*  LT     R.    M.      VERBOS        USN  * 


fc1*  mj*   *JL»^t#*  ■n***p4P*EV  *T-*  *^"*^  »t*  J^»I*  J#J«J«  Jm  MM  »J  *  MM  MM  Jj  MM  MM  JU  MM  MM  MM  M 


-  **J^  *JL*  Jl  te'jrkTdk.1*  w^taJ^  MM  fc"-  ***  »*-  »'■* 


-'-  .<«'.  -v  ■*-  -'-  - '  -  -'/ 


**^ 


m  MM  MM  MM  JL  MM  MM!  Ja  iLwfljL  «**  *t*  *-'^«JL*  »I^  ^ 


U-U 


1 ,    DELPRT  =   .  10 


TERMINAL 

METHOD  ADAMS 

PRINT   .  10,ANGTH2,TH2ANG,ANGTH3,TH3ANG,ANG23 

CONTROL  FINTIM  =4.0,    DELT  =   .0005,    DELMAX  = 

SAVE   .05,    ANGTH2,TH2ANG,ANGTH3,TH3ANG,ANG23 

GRAPH   (DE=TEK618)    TIME , ANGTH2 ,TH2ANG 

GRAPH   (DE=TEK618)   TIME ,ANGTH3 ,TH3ANG 

GRAPH   (DE=TEK618)   TIME,ANG23 

D  DIMENSION  MATA( 27 ,27) ,MASS( 3 ,2) ,L( 3 ,2) ,RX( 3,2) ,RY( 3 ,2) ,RZ( 3,2) 

D  DIMENSION   IXX(3 ,2) , IXZ( 3 ,2) , IXY( 3 ,2) , IYY( 3 ,2) , IYZ( 3,2) ,IZZ( 3 ,2) 

D  DIMENSION   IMAT( 3,3,3) , NIMAT( 3,3) ,TMAT( 3,3) ,TMATTR( 3,3) ,MATTMP( 3,3) 

D  DIMENSION  LIMAT(3,3,3) 

FIXED        IER,I,J,M,K,P,N,IA,IDGT,A,I1 

ARRAY  MATB(27),ABMATB(27),LCOGX(3),LCOGY(3),LCOGZ(3) 

ARRAY  VECTA0(3),VECTB0(3),VECTA1(3),VECTB1(3),VECTA2(3),VECTB2(3) 

ARRAY  VECTA(3),VECTB(3) 

ARRAY  WDX(3),WDY(3),WDZ(3),W1(3),W2(3),W3(3) 

ARRAY  RBG1(3),RAG1(3),RBG2(3),RAG2(3),RBG3(3) 

ARRAY  WKAREA(850) 

ARRAY   IXXT( 3) , IYYTC  3) , IZZT( 3) , IXYT( 3) , IXZT( 3) , IYZT( 3) 

ARRAY  DRCANX(3) ,DRCANY(3) ,DRCANZ(3) 

ARRAY  DRCSX(3),DRCSY(3),DRCSZ(3) 


INITIAL 


***** 
* 
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*** 


INPUT  ***** 

INPUT  PARAMETER  CONSTANTS 
A  =   15.0D0 
P  =  0. 0D0 
W=     2  *  PI 
IDGT  =  6 
MODIFIED  BY     RM  VERBOS 
G  =  9.81D0 
G  =  0.0D0 
N  =  27 
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* 


M  =  1 

IA  =  27 

IER  =  0 

LEVELQ  =  0 

LEVLDQ  =  0 

INPUT  JOINT  LOCATIONS  IN 

METERS 

JXO  =  0.  ODO 

JYO  =  0. ODO 

JZO  =  0.  ODO 

JX1  =  0.  ODO 

JY1  =  0.  ODO 

JZ1  =  O.2D0 

JX2  =  0.  ODO 

JY2  =  0.416D0 

JZ2  =  0.  2D0 

INPUT  TORQUE  CONSTANTS 

TOX    =  0.  ODO 

TOY    =  0. ODO 

TOZ    =  O.ODO 

T1X    =  O.ODO 

T1Y    =  O.ODO 

T1Z    =  0. ODO 

T2X    =  0. ODO 

T2Y    =  0. ODO 

T2Z    =  0. ODO 

INPUT  DISTANCE  FROM  CENTER 

OF 

LINK  TO  CENTER  OF  MASS 

FOR  EACH  LINK  ENDS 

L(l,l)  =  0.  05D0 

L(l,2)  =  0. 15D0 

L(2,l)  =  0. 20D0 

L(2,2)  =  0. 216D0 

L(3,l)  =  0. 225D0 

L(3,2)  =  0.226D0 

*  INPUT  THE  LINK  LENGTHS  OF  THE  ROBOT 
LINKL1  =  0. 20D0 
LINKL2  =  0.416D0 
LINKL3  =  0.451D0 


INPUT  MASS  AT  LINK  ENDS  IN  KILOGRAMS 


110 


120 


MASS(1,1)  = 

11.  ODO 

MASS(1,2)  = 

33. ODO 

MASS(2,1)  = 

2.2D0 

MASS(2,2)  = 

2.2D0 

MASS(3,1)  = 

28.  6D0 

MASS(3,2)  = 

28.6D0 

UT  LOCATION 

OF  LINK  C 

LCOGX(l)  = 

O.ODO 

XI 

LCOGX(l) 

LCOGY(l)  = 

0.  ODO 

Yl 

LCOGY(l) 

LCOGZ(l)  = 

0.  10D0 
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Zl 

= 

LCOGZ(l) 

LC0GX(2) 

= 

0.  ODO 

X2 

= 

LC0GX(2) 

LC0GY(2) 

= 

0. 208D0 

Y2 

= 

LC0GY(2) 

LC0GZ(2) 

= 

0. 20D0 

Z2 

= 

LC0GZC2) 

LC0GX(3) 

s 

0.  ODO 

X3 

:= 

LC0GX(3) 

LC0GY(3) 

= 

0.6415D0 

Y3 

= 

LC0GY(3) 

LC0GZ(3) 

= 

0.  2D0 

Z3 

= 

LC0GZ(3) 

INPUT  MASS  OF  EACH  LINK  IN  KG 
Ml  =  44.  ODO 
M2  =  4.4D0 
M3  =  57.2D0 

INPUT  ACCELERATIONS  OF  JOINT  ZERO 
AXO  =  0.  ODO 
AYO  =  0.  ODO 
AZO  =  0.  ODO 


*      INPUT  THE  INITIAL  DIRECTION  COSINES 

DRCSX(l)  = 

■   0.  ODO 

DRCSY(l)  = 

=  0.  ODO 

DRCSZ(l)  = 

=  1.  ODO 

DRCSX(2)  = 

=  O.ODO 

DRCSY(2)  = 

=  1.  ODO 

DRCSZ(2)  = 

=  0.  ODO 

DRCSX(3)  = 

=  0. ODO 

DRCSY(3)  = 

=  1.  ODO 

DRCSZ(3)  = 

=  0.  ODO 

INPUT  THE  INITIAL  DIRECTION  COSINE 

DRCANX(l) 

=  90.  ODO 

DRCANY(l) 

=  90.  ODO 

DRCANZ(l) 

=   0.  ODO 

DRCANX(2) 

=  90.  ODO 

DRCANY(2) 

=   0.  ODO 

DRCANZ(2) 

=  90. ODO 

DRCANX(3) 

=  90.  ODO 

DRCANY(3) 

=  O.ODO 

DRCANZ(3) 

=  90.  ODO 

*****   INITIALIZE 

***** 

*      OMEGA  AND  OMEGA  DOT 

160       DO  170  I  = 

1,3 

W1(I) 

=  0.  ODO 

W2(I) 

=  0.  ODO 

W3(I) 

=  0.  ODO 

WDX( I ) 

=  0.  ODO 

WDY(I) 

=  0. ODO 

WDZ(I) 

=  0.  ODO 

170       CONTINUE 

ANGLES 
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THZ  =  0.  0D0 


175 
180 


INITIALIZE  MATRIX  A  AND  B  TO  ZERO 
DO  180  I  =  1,27 
DO  175  J  =  1,27 

MATA(I,J)  =  0. 0D0 
CONTINUE 
MATB(I)  =  0. ODO 
CONTINUE 


ftftftftft 
ft 

185 


CALCULATIONS 

WEIGHTS  (NEWTONS) 

WG1  =  M1*G 

WG2  =  M2*G 

WG3  =  M3*G 


ftftftftiY 


COMPUTE  THE  LENGTH  FROM  THE  INBOARD  JOINT  TO  COG 


LNCOG1  =  DSQRT  ( 

LX2  =  LCOGX(2)  - 
LY2  =  LCOGY(2)  - 
LZ2  =  LCOGZ(2)  - 
LNCOG2  =  DSQRT  ( 
LX3  =  LCOGX(3)  - 
LY3  =  LCOGY(3)  - 
LZ3  =  LCOGZ(3)  - 
LNCOG3  =  DSQRT  ( 


LCOGX(l)*LCOGX(l) 

LCOGZ(l)*LCOGZ(l) 

JX1 

JY1 

JZ1 

LX2*LX2  +  LY2*LY2 

JX2 

JY2 

JZ2 

LX3*LX3  +  LY3*LY3 


LCOGY(l)*LCOGY(l)  +. 


+  LZ2*LZ2  ) 


+  LZ3*LZ3  ) 


* 

190 


200 


205 


COMPUTE 
DO 


INITIAL  INERTIAS  BASED 
IN  GLOBAL  COORDINATES 


ON  POINT  MASSES 


1,3 


225    I   = 
RX(I,1)   = 
RX(I,2)   = 
RY(I^)   = 
RY(I,2)    = 
RZ(I,1)   = 
RZ(I,2)    = 
MASS(I 
MASS(I,2) 
IXXT(I)      =   IXX(I,1)    + 


ft 


IXX(I 
IXX(I 


IYY(I 
IYY(I 


IYYT(I)      = 


IZZ(I 
IZZ(I 


IZZT(I)      = 


IXY(I 
IXY(I 


IXYT(I)      = 


IXZ(I 
IXZ(I 


IXZT(I)      = 


IYZ(I 
IYZ(I 


1) 
2) 


-L(I,1) 

L(I,2) 

-L(I,1) 

L(I,2)   * 
-L(I,1)   * 
L(I,2)   * 
1)   *   ((RY( 

((RY(I,2) 
IXX(I,2) 


DRCSX(I) 
DRCSX(I) 
DRCSY(I) 
DRCSY(I) 
DRCSZ(I) 
DRCSZ(I) 
1,1)   *  RY(I 


1) 
2) 


1) 
2) 


1) 
2) 


1) 
2) 


1) 
2) 


MASS(I,1)  *  ((RX(I,1) 
MASS(I,2)  *  ((RX(I,2) 
IYY(I,1)  +  IYY(I,2) 
MASS(I,1)  *  ((RX(I,1) 
MASS(I,2)  *  (CRX(I,2) 
IZZ(I,1)  +  IZZ(I,2) 
MASS(I,1)  *  RX(I,1) 
MASS(I,2)  *  RX(I,2) 
IXY(I.l)  +  IXY(I,2) 
MASS(I,1)   *  RZ(I,1) 

•  RZ(I,2) 
IXZ(I,2) 

•  RY(I,1) 


ft 


* 


* 


IYZT(I)      = 


MASS(I,2)   '' 
IXZ(I,1)   + 
MASS(I,1)   '- 
MASS(I,2)   * 
IYZ(I,1)   + 


RY(I,2) 
IYZ(I,2) 


D) 
RY(I,2)) 

RX(I,1)) 
RX(I,2)) 

RX(I,D) 
RX(I,2)) 


(RZ(I,1) 
(RZ(I,2) 

(RZ(I,1) 

(RZ(I,2) 

(RY(I,1) 
(RY(I,2) 


* 

ft 

ft 
* 


RZ(I,D) 
RZ(I,2)) 

RZ(I,l)) 
RZ(I,2)) 

RY(I,D) 
RY(I,2)) 


RY(l,l) 
RY(I,2) 

RX(I,1) 
RX(I,2) 

RZ(I,1) 
RZ(I,2) 
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225 


* 


IF  (IXXT(I)  .  LE.  .01)  THEN 

IXXT(I)   =  .  01 

ELSE 

IXXT(I)  =  IXXT(I) 

END  IF 

IF  (IYYT(I)  .LE.  .02)  THEN 

IYYT(I)   =  .02 

ELSE 

IYYT(I)   =  IYYT(I) 

END  IF 

IF  (IZZT(I)  .LE.  .02)  THEN 

IZZT(I)   =  .  02 

ELSE 

IZZT(I)   =  IZZT(I) 

END  IF 

IMAT(I,1,1)  =  IXXT(I) 

IMAT(I,1,2)  =   IXYT(I) 

IMAT(I,1,3)  =   IXZT(I) 

IMAT( 1,2,1)  =  -IXYT(I) 

IMAT(I,2,2)  =   IYYT(I) 

IMAT(I,2,3)  =   IYZT(I) 

IMAT(I,3,1)  =  -IXZT(I) 

IMAT(I,3,2)  =  -IYZT(I) 

IMAT(I,3,3)  =   IZZT(I) 

CONTINUE 

DUE  TO  LINK  1  CONSTRAINTS  LINK  1  INERTIAS  ARE  CONSTANT 

IXXT(l)  =  IMAT( 1,1,1) 

IXYT(l)  =  IMAT( 1,1,2) 

IXZT(l)  =  IMATC 1,1,3) 

IYYT(l)  =  IMAT( 1,2,2) 

IYZT(l)  =  IMATC 1,2,3) 

IZZT(l)  =  IMAT( 1,3,3) 

TRANSFORM  THE  INITIAL  INERTIAS  TO  LOCAL  COORDINATED 

DO  9  I  =  2,  3 

TMAT(2,1)  =  -DCOS  (  THZ  ) 

TMAT(2,2)  =  -DSIN  (  THZ  ) 

TMAT(2,3)  =  O.ODO 

TMAT(3,1)  =  DRCSX(I) 

TMAT(3,2)  =  DRCSY(I) 

TMAT(3,3)  =  DRCSZCI) 

VECTA(l)   =  TMAT(2,1) 

VECTA(2)   =  TMAT(2,2) 

VECTAC3)   =  TMATC2,3) 

VECTB(l)   =  TMAT(3,1) 

VECTBC2)   =  TMAT(3,2) 

VECTB(3)   =  TMAT(3,3) 

CALL  CPROD  (VECTA,VECTB,MI1,MJ1,MK1) 

TMAT(1,1)    =  Mil 
TMAT(1,2)   =  MJ1 
TMAT(1,3)    =  MK1 
TMATTR(1,1)    =  TMATC1,1) 
TMATTR(1,2)    =  TMAT(2,1) 
TMATTR(1,3)   =  TMAT(3,1) 
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1 

5 


TMATTR(2,1) 
TMATTR(2,2) 
TMATTR(2,3) 
TMATTR(3,1) 
TMATTR(3,2) 
TMATTR(3,3) 


TMAT(1,2) 
TMAT(2,2) 
TMAT(3,2) 
TMAT(1,3) 
TMAT(2,3) 
TMAT(3,3) 


DO  5  II  =  1,3 
DO  5  J  =1,3 
TEMP  =  0.  ODO 
DO  1   K  =1,3 

TEMP  =  TMAT(I1,K)  *  IMAT(I,K,J)  +  TEMP 
CONTINUE 
MATTMP(I1,J)  =  TEMP 
CONTINUE 


8 
9 


DO  8    II  =  1,3 
DO  8   J  =1,3 
TEMP  =  0. ODO 
DO  7   K  =1,3 

TEMP  =  MATTMP(I1,K)  *  TMATTR(K,J)  +  TEMP 
CONTINUE 
LIMAT(I,I1,J)  =  TEMP 
CONTINUE 
CONTINUE 


*  DOUBLE  PENDULUM 
TH2D0  =  0. ODO 
TH3D0  =  0. ODO 


INITIALIZATION 


TH30   =  PI/ 2. 0 

TH20   =  PI/2. 0 

L2 

=  0.  416D0 

L3 

=  0.451D0 

M2P 

=  30.  8D0 

M3P 

=  28.  6D0 

HF 

=  0.5D0 

DERIVATIVE 

NO SORT 

230 

CALL  ERRSET  (208,256,-1,1,1) 

CALL  UERSET(LEVELQ,LEVLDQ) 

*     INITIALIZE  MATRIX  A  AND  B  TO  ZERO 

DO  240  I  =  1,27 

DO  235  J  =  1,27 

MATA(I,J)  =  0.  ODO 

235 

CONTINUE 

MATB(I)  =  0.  ODO 

240 

CONTINUE 

INPUT  JOINT  EQUATIONS 


JOINT  ZERO  EQUATIONS 
Wl  X  (Wl  X  RB/G1) 

RBGl(l)  =  JXO  -  LCOGX(l) 
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*  RBG1(2)  =  JYO  -  LCOGY(l) 

*  RBG1(3)  =  JZO  -  LCOGZ(l) 

*  VECTA(l)  =  Wl(l) 

*  VECTA(2)  =  Wl(2) 

*  VECTA(3)  =  Wl(3) 

*  CALL  CPROD(VECTAO,RBG1, MICO, MJCO, MKCO) 

*  VECTBO(l)  =  MICO 

*  VECTB0(2)  =  MJCO 

*  VECTB0(3)  =  MKCO 

*  CALL  CPROD(VECTAO,VECTBO, MICO, MJCO, MKCO) 
**     SINCE  LINK  ONE  DOES  NOT  MOVE 

MICO  =  0. ODO 
MJCO  =  0.  ODO 
MKCO  =  0.  ODO 


* 

INPUT  JOINT  EQUATIONS 

* 

JOINT  ZERO  EQUATIONS 

* 

Wl  X  (Wl  X  RB/G1) 

RBG1(1)   =  JXO  -  LCOGX(l) 

RBG1(2)   =  JYO  -  LCOGY(l) 

RBG1(3)   =  JZO  -  LCOGZ(l) 

VECTA(l)  =  Wl(l) 

VECTA(2)  =  Wl(2) 

VECTA(3)  =  Wl(3) 

CALL  CPROD( VECTA , RBG1 , MICO , MJCO , MKCO ) 

VECTB(l)  =  MICO 

VECTB(2)  =  MJCO 

VECTBC3)  =  MKCO 

CALL  CPROD( VECTA, VECTB, MICO, MJCO, MKCO) 

* 

Wl  X  (Wl  X  RA/G1) 

RAG1(1)  =  JX1  -  LCOGX(l) 

RAG1(2)  =  JY1  -  LCOGY(l) 

RAG1(3)  =  JZ1  -  LCOGZ(l) 

VECTA(l)  =  Wl(l) 

VECTA(2)  =  Wl(2) 

VECTA(3)  =  Wl(3) 

CALL  CPROD  (VECTA, RAG1 ,MIC1 ,MJC1 ,MKC1) 

VECTB(l)  =  MIC1 

VECTB(2)  =  MJC1 

VECTB(3)  =  MKC1 

CALL  CPROD  (VECTA, VECTB, MIC1,MJC1,MKC1) 

* 

W2  X  (W2  X  RB/G2) 

RBG2(1)  =  JX1  -  LC0GX(2) 

RBG2(2)  =  JY1  -  LC0GY(2) 

RBG2(3)  =  JZ1  -  LC0GZ(2) 

VECTA(l)  =  W2(l) 

VECTA(2)  =  W2(2) 

VECTA(3)  =  W2(3) 

CALL  CPROD  (VECTA, RBG2 ,MIC2,MJC2 ,MKC2) 

VECTB(l)  =  MIC2 

VECTB(2)  =  MJC2 

VECTB(3)  =  MKC2 

CALL  CPROD  (VECTA, VECTB, MIC2 ,MJC2,MKC2) 

it 

W2  X  (W2  X  RA/G2) 

RAG2(1)  =  JX2  -  LC0GX(2) 

RAG2(2)  =  JY2  -  LC0GY(2) 
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RAG2(3) 
VECTA(l) 
VECTA(2) 
VECTA(3) 

CALL  CPROD 
VECTB(l) 
VECTB(2) 
VECTB(3) 

CALL  CPROD( 
W3  X  (W3  X  RB/G 
RBG3(1) 
RBG3(2) 
RBG3(3) 
VECTA(l) 
VECTA(2) 
VECTA(3) 

CALL  CPROD 
VECTB(l) 
VECTB(2) 
VECTB(3) 

CALL  CPROD 


=  JZ2  -  LCOGZ(2) 

=  W2(l) 

=  W2(2) 

=  W2(3) 
(VECTA,RAG2,MIC3,MJC3,MKC3) 

=  MIC3 

=  MJC3 

=  MKC3 
VECTA,VECTB,MIC3,MJC3,MKC3) 
3) 

=  JX2  -  LCOGX(3) 
=  JY2  -  LC0GY(3) 
=  JZ2  -  LCOGZ(3) 

=  W3(l) 

=  W3(2) 

=  W3(3) 
(VECTA,RBG3,MIC4,MJC4,MKC4) 

=  MIC4 

=  MJC4 

=  MKC4 
(VECTA,VECTB,MIC4,MJC4,MKC4) 


INERTIA  TRANSFORMATION 


DO  390  I  =  2,  3 


TMAT(2,1) 
TflAT(2,2) 
TMAT(2,3) 
TMAT(3,1) 
TMAT( 3,2) 
TMAT( 3,3) 


-DCOS  ( 
-DSIN  ( 
0.  0D0 
DRCSX(I) 
DRCSY(I) 
DRCSZ(I) 


THZ 
THZ 


VECTA(l)  = 
VECTA(2)  = 
VECTA(3)  = 
VECTB(l)  = 
VECTB(2)  = 
VECTB(3)  = 
CALL  CPROD  ( 
TMAT(1,1)  = 
TMAT(1,2)  = 
TMAT(1,3)  = 
TMATTR(1,1) 
TMATTR(1,2) 
TMATTR(1,3) 
TMATTR(2,1) 
TMATTR(2,2) 
TMATTR(2,3) 
TMATTR(3,1) 
TMATTR(3,2) 
TMATTR(3,3) 


TMAT(2,1) 

THAT (2, 2) 

TMAT(2,3) 

TMAT(3,1) 

THAT (3, 2) 

TMAT(3,3) 

VECTA,VECTB,MI1,MJ1,MK1) 

Mil 

MJ1 

MK1 

=  TMAT(1,1) 

=  TMAT(2,1) 

=  TMAT(3,1) 

=  TMAT(1,2) 

=  TMAT(2,2) 

=  TMAT(3,2) 

=  TMAT(1,3) 

=  TMAT(2,3) 

=  TMAT(3,3) 


DO  375  II  =  1,3 
DO  375  J  =  1,3 
TEMP  =  0.  0D0 
DO  370  K  =  1,3 
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TEMP  =  TMATTR(I1,K)   *  LIMAT(I,K,J)   +  TEMP 


370 

CONTINUE 

MATTMP(I1,J)  =  TEMP 

375 

CONTINUE 

DO  380  11  ■ 

=  1,3 

DO  380  J  = 

=  1,3 

TEMP  =  l 

).  ODO 

DO  378  K 

=  1,3 

TEMP  = 

=  MATTMP(I1,K) 

378 

CONTINUE 

NIMAT(I1,. 

J)  =  TEMP 

380 

CONTINUE 

IXXT(I)  = 

NIMAT(1,1) 

IXYT(I)  = 

NIMAT(1,2) 

IXZT(I)  = 

NIMAT(1,3) 

IYYT(I)  = 

NIMAT(2,2) 

IYZT(I)  = 

NIMAT(2,3) 

IZZT(I)  = 

NIMAT(3,3) 

390 

CONTINUE 

VoWc^'oV 

ENTER  CONSTANTS  INTO  MATRIX 

** 

LINK  ONE 

* 

SUM  OF  FORCES 

* 

X  DIRECTION 

395 

MATA(1,1)   = 

1.0D0 

MATA(1,4)   = 

Ml 

MATA(1,10)  = 

-1.  0D0 

MATB(l) 

0.  0D0 

* 

Y  DIRECTION 

MATA(2,2)   = 

1.0D0 

MATA(2,5)   = 

Ml 

MATA(2,11)  = 

-1.0D0 

MATB(2) 

0.  0D0 

Vf 

Z  DIRECTION 

MATA(3,3)   = 

1.  0D0 

MATA(3,6)   = 

Ml 

MATA(3,12)  = 

-1.  ODO 

MATB(3) 

-WG1 

* 

EQUATIONS  AT  JOINT  ZERO 

* 

X  DIRECTION 

MATA(4,4)  = 

1.  ODO 

MATA(4,8)  = 

RBG1(3) 

MATA(4,9)  = 

-RBG1(2) 

MATB(4) 

AXO  -  MICO 

* 

Y  DIRECTION 

MATA(5,5)  = 

1.  ODO 

MATA(5,7)  = 

-RBG1(3) 

MATA(5,9)  = 

RBG1(1) 

MATB(5) 

AYO  -  MJCO 

* 

Z  DIRECTION 

MATA(6,6)  = 

l.ODO 

MATA(6,7)  = 

RBG1(2) 

*  TMAT(K,J)   +  TEMP 
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MATA(6,8)  =  ■ 

MATB(6) 

*  SUM  OF  MOMENTS 
X  DIRECTION 

MATA(7,2)  = 

MATA(7,3)  = 

MATA(7,7)  = 

MATA(7,8)  = 

MATA(7,9)  = 

MATA(7,11)  = 

MATA(7,12)  = 
MATB(7) 

*  Y  DIRECTION 

MATA(8,1)  = 

MATA(8,3)  = 

MATA(8,7)  = 

MATA(8,8)  = 

MATA(8,9)  = 

MATA(8,10)  = 

MATA(8,12)  = 
MATB(8) 
Z  DIRECTION 

MATA(9,1)  = 

MATA(9,2)  = 

MATA(9,7)  = 

MATA(9,8)  = 

MATA(9,9)  = 

MATA(9,10)  = 

MATA(9,11)  = 
MATB(9) 


RBG1(1) 
AZO  -  MKCO 
EQUATIONS 

RBG1(3 
-RBG1(2 
-IXXT(1 

IXYT( 1 

IXZT(1 
-RAG1(3 

RAG1(2 

T1X  -  TOX 


■RBG1(3 
RBG1(1 
IXYTC I 
■IYYT(1 
IYZT(1 
RAG1(3 
■RAG1(1 
T1Y   - 

RBG1(2 
■RBG1(1 

IXZT(1 

IYZT(1 
-IZZT(1 
■RAG1(2 

RAG1(1 

T1Z 


TOY 


TOZ 


460 


* 


* 


LINK  TOO 

SUM  OF  FORCES 

X  DIRECTION 
MATA(10,10) 
MATA(10,13) 
MATA(10,19) 
MATB(IO) 

Y  DIRECTION 
MATA(11,U) 
MATA(11,1^) 
MATA( 11,20) 
MATB(ll) 

Z  DIRECTION 
MATA(12,12) 
MATA(12,15) 
MATA( 12,21) 
MATB(12) 

EQUATIONS  AT 

X  DIRECTION 
MATA(13,4) 
MATA(13,8) 
MATA(13,9) 
MATA(13,13) 
MATA(13,17) 
MATA(13,18) 


=   1.  ODO 
=  M2 
=  -1.  ODO 
=  O.ODO 

=   l.ODO 
=  M2 
=  -l.ODO 
=  0.  ODO 

=   l.ODO 
=  M2 
=  -l.ODO 
=  -WG2 
JOINT  ONE 

=  -l.ODO 
=  -RAG1(3) 
=  RAG1(2) 
=   l.ODO 
=  RBG2(3) 
=  -RBG2(2) 
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MATB(13) 

= 

MIC1  -  MIC2 

*      Y  DIRECTION 

MATA(14,5) 

= 

-l.ODO 

MATA(14,7) 

B 

RAG1(3) 

MATA(14,9) 

= 

-RAGl(l) 

MATA(14,14) 

= 

1.  ODO 

MATA(14,16) 

= 

-RBG2(3) 

MATA(14,18) 

= 

RBG2(1) 

MATB(14) 

= 

MJC1  -  MJC2 

*      Z  DIRECTION 

MATA(15,6) 

= 

-1.  ODO 

MATA(15,7) 

B 

-RAG1(2) 

MATA(15,8) 

= 

RAG1(1) 

MATA(15,15) 

= 

1.  ODO 

MATA(15,16) 

= 

RBG2(2) 

MATA( 15,17) 

B 

-RBG2(1) 

MATB(15) 

B 

MKC1  -  MKC2 

*      SUM  OF  MOMENTS 

;  EQi 

X  DIRECTION 

MATA(16,11) 

B 

RBG2(3) 

MATA(16,12) 

= 

-RBG2(2) 

MATA(16,16) 

B 

-IXXT(2) 

MATA(16,17) 

= 

IXYT(2) 

MATA(16,18) 

= 

IXZT(2) 

MATA( 16,20) 

= 

-RAG2(3) 

MATA( 16,21) 

= 

RAG2(2) 

MATB(16) 

= 

T2X  -  T1X 

*      Y  DIRECTION 

MATA(17,10) 

= 

-RBG2(3) 

MATA( 17,12) 

= 

RBG2(1) 

MATA(17,16) 

= 

IXYT(2) 

MATA(17,17) 

= 

-IYYT(2) 

MATA(17,18) 

B 

IYZT(2) 

MATA(17,19) 

= 

RAG2(3) 

MATA( 17,21) 

= 

-RAG2(1) 

MATB(17) 

B 

T2Y  -  T1Y 

*      Z  DIRECTION 

MATA( 18,10) 

= 

RBG2(2) 

MATA(18,11) 

= 

-RBG2(1) 

MATA(18,16) 

B 

IXZT(2) 

MATA(18,17) 

= 

IYZT(2) 

MATA(18,18) 

B 

-IZZT(2) 

MATA(18,19) 

= 

-RAG2(2) 

MATA( 18,20) 

= 

RAG2(1) 

MATB(18) 

~ 

T2Z  -  T1Z 

**     LINK  THREE 

*      SUM  OF  FORCES 

*      X  DIRECTION 

530      MATA( 19,19) 

= 

1.  ODO 

MATA( 19,22) 

= 

M3 

MATB(19) 

= 

O.ODO 

*      Y  DIRECTION 

MATA(20,20) 

= 

l.ODO 

MATA(20,23) 

S 

M3 
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* 


* 
* 


* 


* 


MATB(20) 

Z  DIRECTION 
MATA(21,21)  = 
MATA(21,24)  = 
MATB(21)  =  -W 

EQUATIONS  AT  JO 

X  DIRECTION 
MATA(22,13)  = 
MATA(22,17)  = 
MATA(22,18)  = 
MATA(22,22)  = 
MATA(22,26)  = 
MATA( 22,27)  = 
MATB(22) 

Y  DIRECTION 
MATA(23,14)  = 
MATA(23,16)  = 
MATA(23,18)  = 
MATA(23,23)  = 
MATA(23,25)  = 
MATA(23,27)  = 
MATB(23) 

Z  DIRECTION 
MATA(24,15)  = 
MATA(24,16)  = 
MATA(24,17)  = 
MATA(24,24)  = 
MATA(24,25)  = 
MATA( 24,26)  = 
MATB(24) 

SUM  OF  MOMENTS 

X  DIRECTION 
MATA(25,20) 
MATA(25,21) 
MATA(25,25) 
MATA(25,26) 
MATA(25,27) 
MATB(25) 

Y  DIRECTION 
MATA(26,19) 
MATA(26,21) 
MATA(26,25) 
MATA(26,26) 
MATA(26,27) 
MATB(26) 

Z  DIRECTION 
MATA(27,19) 
MATA(27,20) 
MATA(27,25) 
MATA(27,26) 
MATA(27,27) 
MATB(27) 


=  0.  0D0 


1.0D0 
M3 
G3 

INT  TWO 

-1.  ODO 
-RAG2(3) 

RAG2(2) 

l.ODO 

RBG3(3) 
-RBG3(2) 

MIC3  -  MIC4 

-l.ODO 

RAG2(3) 
-RAG2(1) 

1.  ODO 
-RBG3(3) 

RBG3(1) 

MJC3  -  MJC4 

-1.  ODO 
-RAG2(2) 

RAG2(1) 

1.  ODO 

RBG3(2) 
-RBG3(1) 

MKC3  -  MKC4 
EQUATIONS 

RBG3(3) 
-RBG3(2) 
-IXXT(3) 

IXYT(3) 

IXZT(3) 
-T2X 

-RBG3(3) 

RBG3(1) 

IXYT(3) 
-IYYT(3) 

IYZT(3) 
-T2Y 

RBG3(2) 
-RBG3(1) 

IXZT(3) 

IYZT(3) 
-IZZT(3) 
-T2Z 


*****   FIRST  LINK  IS  CONSTRAINED  TO  ROTATE 
590      DO  600  I  =  4,8 

DO  595  J  =  1,27 


IN  Z  DIRECTION   ***** 
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w 

595 

600 


605 
610 


j..'.j. 


MATA( I 

,J) 

= 

0.0D0 

MATA(J 

,D 

= 

0.  0D0 

CONTINUE 

MATB(I" 

)  = 

0. 

0D0 

CONTINUE 

MATA(4 

A) 

= 

1.  0D0 

MATA(5 

,5) 

= 

l.ODO 

MATA(6 

,6) 

zz 

1.  ODO 

MATA( 7 

,7) 

= 

1.  ODO 

MATA( 8 , 

,8) 

ss 

l.ODO 

DO  610  J  = 

1,27 

MATA(18 

,J) 

= 

O.ODO 

MATA(27 

,J) 

= 

0.  ODO 

CONTINUE 

MATA( 9,9' 

) 

s 

-(IZZT(1)+IZZT(2)+IZZT(3)) 

MATA(18,? 

0 

= 

-1.  ODO 

MATA(18,: 

L8) 

= 

1.  ODO 

MATB(18) 

ss 

0.  ODO 

MATA(27,< 

n 

= 

-l.ODO 

MATA(27,: 

17) 

= 

1.  ODO 

MATB(27) 

= 

0.  ODO 

*      CALL  EQUATION  SOLVER  PROGRAM  FROM  IMSL 

620        CALL  LEQT2F(MATA,M,N,IA,MATB,IDGT,WKAREA,IER) 

IF  (IER.  EQ.  0)  THEN 

GOTO  640 

ELSE 

WRITE  (*,624)  IER 
624        FORMAT  (15) 

DO  635   I  =  1,  27 

WRITE  (*,627)  I 
627        FORMAT  (17) 

DO  631   J  =  1,  27,  3 

WRITE  (*,630)  J,MATA(I,J),J+l,MATA(I,J+l),J+2,MATA(I,J+2) 

630  FORMAT  ( 15 ,F13. 5 , 15 ,F13. 5 , 15 ,F13. 5) 

631  CONTINUE 

WRITE  (*,633)  I,MATB(I) 
633        FORMAT  (I3.F15. 5) 
635        CONTINUE 

CALL  ENDJOB 

END  IF 


FIND  LCOGX,LCOGY,LCOGZ,THETA  VALUES ,WX,WY,WZ 
***    MODIFIED  BY  R. M. VERBOS 

*  JOINT  ZERO 

640  FXO  =  MATB(l) 
FYO  =  MATB(2) 
FZO  =  MATB(3) 

*  LINK  ONE 

*  SINCE  LINK1  IS  CONSTRAIN  NOT  TO  MOVE  ALL  ACC  AND  VEL  ARE  ZERO 

AX1  =  0.  ODO 
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AY1  =  0. 

0D0 

AZl  =  0. 

0D0 

*660 

AX1 

=  MATB(4) 

* 

VELX1 

=  INTGRL(0.  ,AX1) 

* 

LCOGX1 

=  1NTGRL(X1,VELX1) 

* 

LCOGX(l) 

=  LC0GX1 

-v 

AY1 

=  MATB(5) 

* 

VELY1 

=  INTGRL(0.  ,AY1) 

* 

LCOGY1 

=  INTGRL(Y1,VELY1) 

* 

LCOGY(l) 

=  LC0GY1 

AZl 

=  MATB(6) 

* 

VELZ1 

=  INTGRL(0.  ,AZ1) 

JU 
7i 

LCOGZ1 

=  INTGRL(Z1,VELZ1) 

* 

LCOGZ(l) 

=  LCOGZ1 

* 

WD  IX 

=  MATB(7) 

* 

WIX 

=  INTGRL(0.  ,WD1X) 

* 

WDX(l) 

=  WD  IX 

* 

Wl(l) 

=  WIX 

JL. 

WD1Y 

=  MATB(8) 

* 

W1Y 

=  INTGRL(0. ,WD1Y) 

*v 

WDYC 1 ) 

=  WD1Y 

Vf 

Wl(2) 

=  W1Y 

* 

WD1Z 

=  MATB(9) 

•jV 

W1Z 

=  INTGRL(0.  ,WD1Z) 

°v 

WDZ(l) 

=  WD1Z 

* 

Wl(3) 

=  W1Z 

.',  J.  -t- 

ADDED  BY 

R.  M.  VERBOS 

685 

THZ 

=  INTGRL(0.  ,W1Z) 

THZANG 

=  THZ  *  RADEG 

COSTHZ 

=  DCOS(THZ) 

SINTHZ 

=  DSIN(THZ) 

IF  THE  1ST  LINK  IS  CONSTRAIN  TO  ROTATE  IN  THE  Z  DIRECTION  ONLY 
THE  DIRECTION  COSINE  AND  DIRECTION  COSINE  ANGLES  ARE  CONSTANT 
AND  DO  NOT  NEED  TO  BE  CALCULATED 


''-700 


* 

Vf 

* 
* 

*720 

JU 


CALC  DIRECTIONAL  COSINES  FOR  LINK  ONE 

DRCSX(l)   =  LCOGX1  /  LNCOG1 

DRCSY(l)   =  LCOGY1  /  LNCOG1 

DRCSZ(l)   =  LCOGZ1  /  LNCOG1 
AMP  =  DSQRT(DRCSX(1)*DRCSX(1)+DRCSY(1)*DRCSY(1)+. 

DRCSZ(1)*DRCSZ(1)) 
DRCSX(l)  =  DRCSX(1)/AMP 
DRCSY(l)  =  DRCSY(1)/AMP 
DRCSZ(l)  =  DRCSZ(1)/AMP 
DRCANX(l)  =  DACOS(DRCSX(l)) 
DRCANY(l)  =  DACOS(DRCSY(l)) 
DRCANZ(l)  =  DACOS(DRCSZ(l)) 


* 


■>'<• 


RADEG 
RADEG 
RADEG 


**  JOINT  LOCATION 

-730  JX1  =  LINKL1 

*  JY1  =  LINKL1 

*  JZ1  =  LINKL1 


*  DRCSX(l) 

*  DRCSY(l) 

*  DRCSZ(l) 
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*      JOINT  ONE 

735      FX1  =  MATB(IO) 

FY1  =  MATB(ll) 

FZ1  =  MATB(12) 

**     LINK  TWO 

740      AX2 

= 

MATB(13) 

VELX2 

= 

INTGRL(0. 

,AX2) 

LC0GX2 

= 

INTGRL(X2 

,VELX2) 

LC0GX(2) 

= 

LC0GX2 

AY2 

= 

MATB(14) 

VELY2 

= 

INTGRL(0. 

,AY2) 

LC0GY2 

= 

INTGRL(Y2 

,VELY2) 

LC0GY(2) 

SB 

LC0GY2 

AZ2 

= 

MATB(15) 

VELZ2 

= 

INTGRL(0. 

,AZ2) 

LC0GZ2 

= 

INTGRL(Z2 

,VELZ2) 

LC0GZ(2) 

= 

LC0GZ2 

WD2X 

= 

MATB(16) 

V2X 

= 

INTGRL(0. 

,WD2X) 

WDX(2) 

= 

WD2X 

W2(l) 

= 

W2X 

WD2Y 

= 

MATB(17) 

W2Y 

= 

INTGRL(0. 

,WD2Y) 

WDY(2) 

= 

VD2Y 

W2(2) 

= 

W2Y 

WD2Z 

= 

MATB(18) 

W2Z 

= 

INTGRL(0. 

,WD2Z) 

WDZ(2) 

= 

WD2Z 

W2(3) 

= 

W2Z 

*  GET  THE  DIRECTION  COSINES  FOR  THE  LINK  TWO 

DRCSX(2)   =  (LC0GX2  -  JX1)  /  LNC0G2 
DRCSY(2)   =  (LC0GY2  -  JY1)  /  LNC0G2 
DRCSZ(2)   =  (LC0GZ2  -  JZ1)  /  LNC0G2 
790      AMP  =  DSQRT(DRCSX(2)*DRCSX(2)+DRCSY(2)*DRCSY(2)+. 

DRCSZ(2)*DRCSZ(2)) 
DRCSX(2)  =  DRCSX(2)/AMP 
DRCSY(2)  =  DRCSY(2)/AMP 
DRCSZ(2)  =  DRCSZ(2)/AMP 
DRCANX(2)  =  DAC0S(DRCSX(2))  *  RADEG 
DRCANY(2)  =  DAC0S(DRCSY(2))  *  RADEG 
DRCANZ(2)  =  DAC0S(DRCSZ(2))  *  RADEG 

*  JOINT  LOCATION 

800  JX2  =  JX1  +  LINKL2  *  DRCSX(2) 
JY2  =  JY1  +  LINKL2  *  DRCSY(2) 
JZ2  =  JZ1  +  LINKL2  *  DRCSZ(2) 

*  JOINT  TWO 

805  FX2  =  MATB(19) 
FY2  =  MATB(20) 
FZ2  =  MATB(21) 


**     LINK  THREE 
812      AX3 

VELX3 


=  MATB(22) 

=  INTGRL(0.  ,AX3) 
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LC0GX3 

LC0GX(3) 

AY3 

VELY3 

LC0GY3 

LC0GY(3) 

AZ3 

VELZ3 

LC0GZ3 

LC0GZ(3) 

WD3X 

W3X 

WDX(3) 

W3(l) 

VD3Y 

W3Y 

WDY(3) 

W3(2) 

VD3Z 

W3Z 

VDZ(3) 

W3(3) 


,AY3) 
,VELY3) 


,AZ3) 
,VELZ3) 


INTGRL(X3,VELX3) 

LC0GX3 

MATB(23) 

INTGRL(0. 

INTGRL(Y3 

LC0GY3 

MATB(24) 

INTGRL(0. 

INTGRL(Z3, 

LC0GZ3 

MATB(25) 

INTGRL(0.  ,WD3X) 

WD3X 

W3X 

MATB(26) 

INTGRL(0.  ,WD3Y) 

WD3Y 

W3Y 

MATBC27) 

INTGRL(0.  ,WD3Z) 

WD3Z 

W3Z 


*  CALC  DIRECTIONAL  COSINES  FOR  LINK  THREE 

845  DRCSX(3)  =  (LCOGX3  -  JX2)  /  LNCOG3 
DRCSY(3)  =  (LC0GY3  -  JY2)  /  LNCOG3 
DRCSZ(3)  =  (LCOGZ3  -  JZ2)  /  LNCOG3 

865      AMP  =  DSQRT(DRCSX(3)*DRCSX(3)+DRCSY(3)*DRCSY(3)+. 

DRCSZ(3)*DRCSZ(3)) 
DRCSX(3)  =  DRCSX(3)/AMP 
DRCSY(3)  =  DRCSY(3)/AMP 
DRCSZ(3)  =  DRCSZ(3)/AMP 
DRCANX(3)  =  DACOS(DRCSX(3))  *  RADEG 
DRCANY(3)  =  DACOS(DRCSY(3))  *  RADEG 
DRCANZ(3)  =  DACOS(DRCSZ(3))  *  RADEG 


TIP 


*875 


LOCATION 
TIPX  = 
TIPY  = 
TIPZ  = 


JX2 
JY2 
JZ2 


+  LINKL3  *  DRCSX(3) 
+  LINKL3  *  DRCSY(3) 
+  LINKL3  *  DRCSZ(3) 


880 


FIND  THE  ANGLE  BETWEEN  THE  LIMKS 
ANG23X  =  DRCANXC2)  -  DRCANX(3) 
ANG23Y  =  DRCANY(2)  -  DRCANY(3) 
ANG23Z  =  DRCANZ(2)  -  DRCANZ(3) 
ANG23  =  ANG23X  +  ANG23Y  +  ANG23Z 


ANG12X  =  DRCANX(l)  -  DRCANX(2) 
ANG12Y  =  DRCANY(l)  -  DRCANY(2) 
ANG12Z  =  DRCANZ(l)  -  DRCANZ(2) 


*     DOUBLE  PENDULUM  CALCULATION 

PA     =  L3*M3P*SIN(TH2-TH3)*TH3DOT**2 
PB     =  SIN(TH2)*G*(M3P+M2P) 
PC     =  L3''"M3P^COS(TH2-TH3) 
PD     =  L2"-'-(M3P+M2P) 


78 


PE     =  L2*SIN(TH2-TH3)*TH2D0T**2 

PF     =  SIN(TH3)*G 

PG     =  C0S(TH2-TH3)*L2 

PH     =  L3 

TH3DDT  =  (PE-PF+((PG*PA+PG*PB)/PD))/(PH-(PG*PC/PD)) 

TH2DDT  =  (-PA  -  PB  -  PC*TH3DDT)/PD 

TH3D0T  =  INTGRL(TH3D0,TH3DDT) 

TH2D0T  =  INTGRL(TH2D0,TH2DDT) 

TH3    =  INTGRL(TH30,TH3D0T) 

TH2    =  INTGRL(TH20,TH2DOT) 

TH2ANG  =  TH2  *  RADEG 

IF(TH2ANG.  GT.  0.  0)THEN 

ANGTH2  =  180.0  -  DRCANZ(2) 

ELSE 

ANGTH2  =  DRCANZ(2)  -  180.0 
ENDIF 

TH3ANG  =  TH3  *  RADEG 
IF(TH3ANG.  GT.  0.  0)THEN 

ANGTH3  =  180.0  -  DRCANZ(3) 

ELSE 

ANGTH3  =  DRCANZ(3)  -  180.0 
ENDIF 

END 
STOP 

FORTRAN 

*     SUBROUTINE  TO  COMPUTE  THE  CROSS  PRODUCT  OF  TWO  VECTORS 


940 


SUBROUTINE  CPROD( VECTA , VECTB , MI , MJ , MK) 
IMPLICIT  REAL*8  (A-Z) 
DIMENSION  VECTA(3),VECTB(3) 

MI  =  VECTA(2)  *  VECTB(3)  -  VECTA(3)  *  VECTB(2) 
MJ  =  VECTA(3)  *  VECTB(l)  -  VECTA(l)  *  VECTB(3) 
MK  =  VECTA(l)  *  VECTB(2)  -  VECTA(2)  *  VECTB(l) 
RETURN 


END 
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APPENDIX  C 
THREE  DIMENSION  VALIDATION  PROGRAM 


TITLE  NONSINGULAR  3-D  VALIDATION  PROGRAM 

*  THIS   IS  A  PROGRAM  WHICH  VALIDATES  THE  NON-SINGULARITY  OF  A  THREE  * 

*  LINK  RIGID  BODY  MANIPULATOR  WITHOUT  THE  USE  OF  THE   STANDARD  ROBOT  * 

*  TRANSFORMATION  MATRICES.      THE   ORIGINAL  PROGRAM  WAS  WRITTEN   BY  LT  * 

*  ATINOK  AND  HAS   BEEN  GREATLY  MODIFIED  TO   INCLUDE  THREE  DIMENSIONAL  * 

*  MOTION  AND  GRAVITY   BY        LT     ROBERT  M.    VERBOS.                    SEPTEMBER   1988  * 

M    <  >    /»    M   ft   rt   » t   ft    M   "   #5   7%  *»    '»   *t  if   ft  ft  31  *      «t   *T  t%    .  ,    * .    /i   ft   f»    .  i   (i    ,  .    * .    «*   *  i    *t  ft   t\i\  7i    «t  ft   ft   ft   ft  ft  «t  ft  7f  »f  i\    /  \    jv    n   ftTt  ft  ft  >(ft  ft   ft  ft   ft  ft   ft   ft  ft7t   ft  ft  ft  *f  7t 

TERMINAL 

METHOD  ADAMS 

PRINT  . 02,ERROR1,ERROR2,ERROR3,    ANG12,    ANG23 

CONTROL  FINTIM  =1.6,    DELT  =  .0005,    DELMAX  =  .1,    DELPRT  =  .02 

D  DIMENSION  MATA( 27 ,27) ,MASS( 3,2) ,L(3 ,2) ,RX( 3 ,2) ,RY(3,2) ,RZ( 3 ,2) 

D  DIMENSION   IXX( 3,2) , IXZ( 3,2) ,IXY( 3 , 2) , IYY( 3 ,2) , IYZ( 3, 2) , IZZ( 3,2) 

D  DIMENSION   IMAT( 3 , 3 , 3) ,NIMAT( 3,3) ,TMAT( 3,3) ,TMATTR( 3,3) ,MATTMP( 3,3) 

D  DIMENSION  LIMAT(3,3,3) 

FIXED        IER,I,J,M,K,P,N,IA,IDGT,A,I1 

ARRAY  MATB(27),MATC(27),MATD(27),LCOGX(3),LCOGY(3),LCOGZ(3) 

ARRAY  VECTA(3),VECTB(3) 

ARRAY  WDX(3),WDY(3),WDZ(3),W1(3),W2(3),W3(3) 

ARRAY  RBG1(3),RAG1(3),RBG2(3),RAG2(3),RBG3(3) 

ARRAY  WKAREA(850) 

ARRAY   IXXT( 3) , IYYT( 3) , IZZT( 3) , IXYT( 3) , IXZT( 3) , IYZT(3) 

ARRAY  DRCANX(3),DRCANY(3),DRCANZ(3) 

ARRAY  DRCSX(3),DRCSY(3),DRCSZ(3), 

INITIAL 

-j-  -'-  -■-  -•-  -j-  j  vjpT  irv  VfVf^t'yr'fV 

*  INPUT  PARAMETER  CONSTANTS 
55  A  15.0D0 

P         =0.  0D0 

W         =     PI/2.0 

IDGT  =     6 

G         =     9.81D0 

N         =  27  ) 

M         =   1 

IA       =  27 

IER     =  0 

LEVELQ  =  0 

LEVLDQ  =  0 

*  INPUT  JOINT  LOCATIONS   IN  METERS 

JXO  =  0.  0D0 
JYO  =  0.  0D0 
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* 


* 
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90 


* 


* 


JZO  =  0. ODO 

JX1  =  0.  ODO 

JY1  =  0.  ODO 

JZ1  =  0.  2D0 

JX2  =  0.  ODO 

JY2  =  0.416D0 

JZ2  =  0.  2D0 

INPUT  DISTANCE 

FROM  CENTER  OF  LINK  T 

L(l,l)  =  0.( 

D5D0 

L(l,2)  =  o. : 

L5D0 

L(2,l)  =  o. : 

>08D0 

L(2,2)  =  o. : 

Z08D0 

L(3,l)  =  o. : 

Z255D0 

L(3,2)  =  o. : 

Z255D0 

INPUT  THE  LINK  J 

LENGTHS 

LINKL1  =  0. : 

ZODO 

LINKL2  =  0.< 

416D0 

LINKL3  =  0. ' 

+51D0 

INPUT  MASS  AT  ] 

LINK  ENDS  IN  KILOGRAMS 

MASS(1,1)  = 

1.  ODO 

MASS(1,2)  = 

3.  ODO 

MASS(2,1)  = 

2.  2D0 

MASS(2,2)  = 

2.2D0 

MASS(3,1)  = 

8.  6D0 

MASS(3,2)  = 

8.  6D0 

INPUT  LOCATION 

OF  LINK  CENTERS  OF  GR 

LCOGX(l)  = 

0.  ODO 

XI 

LCOGX(l) 

LCOGY(l)  = 

0.  ODO 

Yl 

LCOGY(l) 

LCOGZ(l)  = 

0.  10D0 

Zl 

LCOGZ(l) 

LC0GX(2)  = 

0.  ODO 

X2 

LC0GX(2) 

LC0GY(2)  = 

0. 208D0 

Y2 

LC0GY(2) 

LC0GZ(2)  = 

0.  20D0 

Z2 

LC0GZ(2) 

LC0GX(3)  = 

0.  ODO 

X3 

LC0GX(3) 

LC0GY(3)  = 

0.  6415D0 

Y3 

LC0GY(3) 

LC0GZ(3)  = 

0.  2D0 

Z3 

LC0GZ(3) 

INPUT  MASS  OF  \ 

EACH  LINK  IN  KG 

Ml  =  4.  ODO 

M2  =  4.  4D0 

M3  =  17.  2D0 

INPUT  ACCELERATIONS  OF  JOINT  ZERO 

AXO  =  0. ODO 

AYO  =  0.  ODO 

AZO  =  0. ODO 

INPUT  THE  INITIAL  DIRECTION  COSINES 

DRCSX(l)  = 

0.  ODO 

DRCSY(l)  = 

0.  ODO 

DRCSZ(l)  = 

1.  ODO 
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DRCSX(2)  =  0. ODO 

DRCSY(2)  =  1. ODO 

DRCSZ(2)  =  0. ODO 

DRCSX(3)  =  0. ODO 

DRCSY(3)  =  1.  ODO 

DRCSZ(3)  =  O.ODO 

* 

INPUT  THE  INITIAL  DIRECTION  COSINE  ANGLES 

DRCANX(l)  =  90. ODO 

DRCANY(l)  =  90. ODO 

DRCANZ(l)  =  O.ODO 

DRCANX(2)  =  90. ODO 

DRCANY(2)  =   O.ODO 

DRCANZ(2)  =  90. ODO 

DRCANX(3)  =  90. ODO 

DRCANY(3)  =   0.  ODO 

DRCANZ(3)  =  90. ODO 

***** 

INITIALIZE   ***** 

■sV 

OMEGA  AND  OMEGA  DOT 

139 

DO  146  I  =  1,3 
W1(I)   =  0.  ODO 
W2(I)   =  O.ODO 
W3(I)   =  0.  ODO 
VDX(I)  =  0.  ODO 
WDY(I)  =  0.  ODO 
WDZ(I)  =  O.ODO 

146 

CONTINUE 

Wl(3)   =  2.  ODO 
W2(l)   =  2.  ODO 
W3(l)   =  2.  ODO 
WDY(2)  =  4.  ODO 
WDY(3)  =  4.  ODO 

THZ    =  0.  ODO 

* 

INITIALIZE  MATRICES  TO  ZERO 
DO  180  I  =  1,27 
DO  175  J  =  1,27 

MATA(I,J)  =  0. ODO 

175 

CONTINUE 
MATB(I)  =  0. ODO 
MATC(I)  =  0. ODO 
MATDU)  =  0.  ODO 

180 

CONTINUE 

*****  CALCULATIONS    ***** 

*  WEIGHTS  (NEWTONS) 
185        WG1  =  M1*G 

WG2  =  M2*G 
WG3  =  M3*G 

*  COMPUTE  THE  LENGTH  FROM  THE  INBOARD  JOINT  TO  COG 

LNC0G1  =  DSQRT  (  LCOGX( 1)*LC0GX( 1)  +  LCOGY( 1)*LC0GY( 1)  +. 

LC0GZ(1)*LC0GZ(1)  ) 
LX2  =  LC0GX(2)  -  JX1 
LY2  =  LC0GY(2)  -  JY1 
LZ2  =  LC0GZ(2)  -  JZ1 
LNC0G2  =  DSQRT  (  LX2--LX2  +  LY2*LY2  +  LZ2*LZ2  ) 
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ft 

190 


200 


205 


225 


LX3  =  LCOGX(3)  - 
LY3  =  LCOGY(3)  - 
LZ3  =  LC0GZ(3)  - 
LNCOG3  =  DSQRT  ( 
COMPUTE  FBD  INERTIAS 


JX2 

JY2 

JZ2 

LX3*LX3  +  LY3*LY3  +  LZ3*LZ3  ) 

BASED  ON  POINT  MASSES  IN  GLOBAL  COORDINATES 


DO 


1,3 


■L(I,1) 

L(I,2) 

■L(I,1) 

L(I,2) 

■L(I,1) 

L(I,2) 


MASS(I.l)  " 
MASS(I,2)  '' 
IXX(I,1)  + 
MASS(I,1)  1 
MASS(I,2)  * 
IYY(I,1)  + 
MASS(I.l)  '' 


MASS(I,2)  *  ((RX(I,2) 
IZZ(I,1)  +  IZZ(I,2) 
MASS(I,1)  *  RX(I,1)  * 
MASS(I,2)  *  RX(I,2)  * 
IXY(I,1)  +  IXY(I,2) 
MASS(I,1)  *  RZ(I,1)  * 
MASS(I,2)  *  RZ(I,2) 
IXZ(I,1)  +  IXZ(I,2) 
MASS(I,1)  *  RY(I,1) 
MASS(I,2)  *  RY(I,2) 
IYZ(I,2) 


225  I  = 

RX(I,1) 

RX(I,2) 

RY(I,1) 

RY(I,2) 

RZ(I,1) 

RZ(I,2) 
IXX(I,1) 
IXX(I,2) 
IXXT(I) 
IYY(I,1) 
IYY(I,2) 
IYYT(I) 
IZZ(I,1) 
IZZ(I,2) 
IZZT(I) 
IXY(I,1) 
IXY(I,2) 
IXYT(I) 
IXZ(I,1) 
IXZ(I,2) 
IXZT(I) 
IYZ(I,1) 
IYZ(I,2) 

IYZT(I)   =  IYZ(I, 
IF  (IXXT(I)  .  LE. 
IXXT(I)   =  .  01 
ELSE 

IXXT(I)  =  IXXT(I) 
END  IF 

IF  (IYYT(I)  .LE. 
IYYT(I)   =  .01 
ELSE 

IYYT(I)   =  IYYT(I) 
END  IF 

IF  (IZZT(I)  .LE 
IZZT(I)   =  .  01 
ELSE 

IZZT(I)   =  IZZT(I) 
END  IF 

IMAT(I,1,1)  = 
IMAT(I,1,2)  = 
IMAT(I,1,3)  = 
IMAT(I,2,1)  = 
IMAT(I,2,2)  = 
IMAT(I,2,3)  = 

IMAT(I,3,1)  = 
IMAT(I,3,2)  = 
IMAT(I,3,3)  = 

CONTINUE 
DUE  TO  LINK  1  CONSTRAINTS 


■k 
ft 


((RY(I,1) 
((RY(I,2) 

IXX(I,2) 
((RX(I,1) 
((RX(I,2) 

IYY(I,2) 
((RX(I,1) 


DRCSX(I) 

DRCSX(I) 

DRCSY(I) 

DRCSY(I) 

DRCSZ(I) 

DRCSZ(I) 

RY(I,1)) 
RY(I,2)) 


?v 


ft 


1)     + 

.01) 


THEN 


01)   THEN 


01)    THEN 


IXXT(I) 

IXYT(I) 

IXZT(I) 

-IXYT(I) 

IYYT(I) 

IYZT(I) 

-IXZT(I) 

-IYZT(I) 

IZZT(I) 


RX(I,1)) 
RX(I,2)) 

RX(I,1)) 
RX(I,2)) 


(RZ(I,1) 
(RZ(I,2) 

(RZ(I,1) 
(RZ(I,2) 

(RY(I,1) 

(RY(I,2) 


ft 

ft 
ft 


RZ(I,D) 
RZ(I,2)) 

RZ(Isl)) 
RZ(IS2)) 

RY(I,1)) 
RY(I,2)) 


ft 


ft 


RY(I,1) 
RY(I,2) 

RX(I,1) 
RX(I,2) 

RZ(I,1) 
RZ(I,2) 


AND  SYMMETRY  LINK  1  INERTIAS  ARE  CONST 
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IXXT(l)  = 

IXYT(l)  = 

IXZT(l)  = 

IYYT(l)  = 

IYZT(l)  = 

IZZT(l)  = 
TRANSFORM  THE 

DO  281  I  =  2 
TMAT(2,1)  ■ 
TMAT(2,2) 
TMAT(2,3)  : 
TMAT(3,1)  i 
TMAT(3,2) 
TMAT(3,3) 
VECTA(l) 
VECTA(2) 
VECTA(3) 
VECTB(l) 
VECTB(2) 
VECTB(3) 
CALL  CPROD 
TMAT(1,1)  = 
TMAT(1,2)  = 
TMAT(1,3)  = 
TMATTR(1,1) 
TMATTR(1,2) 
TMATTR(.1,3) 
TMATTR(2,1) 
TMATTR(2,2) 
TMATTR(2,3) 
TMATTR(3,1) 
TMATTR(3,2) 
TMATTR(3,3) 
DO  272  II  = 
DO  272  J  = 
TEMP  =  0. 
DO  270  K 


IMAT( 1,1,1) 
IMAT( 1,1,2) 
IMAT( 1,1,3) 
IMAT( 1,2,2) 
IMAT( 1,2,3) 
IMAT( 1,3,3) 
GLOBAL  INERTIAS 


TO  LOCAL  WHICH  REMAIN  CONSTANT 


:70 


272 


278 

280 
281 


3 
:  -DCOS  (  THZ  ) 
=  -DSIN  (  THZ  ) 
=  0.  0D0 

■  DRCSX(I) 
=  DRCSY(I) 
:  DRCSZ(I) 
■■   TMAT(2,1) 

■  TMAT(2,2) 

■  TMAT(2,3) 

■  TMAT(3,1) 

■  TMAT(3,2) 
=  TMAT(3,3) 

(VECTA,VECTB,MI1,MJ1,MK1) 
Mil 
MJ1 
MK1 

=  TMAT(1,1) 
=  TMAT(2,1) 
=  TMAT(3,1) 
=  TMAT(1,2) 
=  TMAT(2,2) 
=  TMAT(3,2) 
=  TMATf 1,3) 
=  TMAT(2,3) 
=  TMAT(3,3) 
1,3 
1,3 
0D0 
=  1,3 


=  TMAT(I1,K)  *  IMAT(I,K,J)  +  TEMP 


CONTINUE 
MATTMP(I1,J)  =  TEMP 
CONTINUE 

DO  280    II  =  1,3 
DO  280   J  =1,3 
TEMP  =  0.  0D0 
DO  278   K  =  1,3 

TEMP  =  MATTMP(I1,K) 
CONTINUE 
LIMAT(I,I1,J)  =  TEMP 
CONTINUE 
CONTINUE 


*  TMATTR(K,J)  +  TEMP 


DERIVATIVE 

NOSORT 

287 


CALL  ERRSET  (208,256,-1,1,1) 
CALL  UERSET(LEVELQ,LEVLDQ) 
INITIALIZE  MATRICES  TO  ZERO 
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293 


297 


DO  297  I  =  1,27 
DO  293  J  =  1,27 
MATA(I,J)  =  0. 
CONTINUE 


0D0 


MATB(I) 
MATC(I) 
MATD(I) 
CONTINUE 


=  0.  0D0 
=  0.  ODO 
=  0.  ODO 


* 


VALIDATION  PORTION  OF  PROGRAM  *** 
ENTER  THE  ANGULAR  AND  LINEAR  ACCELERATIONS,  AND  FORCES  INTO  MATRIX  C 
FOR  THE  CONSTRAIN  OF  THE  NEPTUNE  II 


TH1L 

= 

2.  0  *  TIME  +  PI/2.  0 

W1L 

= 

2.  ODO 

WD1L 

= 

0.  ODO 

TH1ANG 

= 

TH1L  * 

RADEG 

WDX1G 

= 

0.  ODO 

WDY1G 

= 

0.  ODO 

WDZ1G 

= 

WD1L 

WX1G 

= 

O.ODO 

WY1G 

= 

0.  ODO 

WZ1G 

= 

W1L 

TH2L 

= 

2.  0  *  TIME 

W2L 

= 

2.  0 

WD2L 

= 

0.  ODO 

TH2ANG 

= 

TH2L  * 

RADEG 

WX2G 

= 

W2L  *  i 

5IN(TH1L) 

WY2G 

= 

W2L  *  I 

>  COS(THIL)  ) 

WZ2G 

= 

W1L 

VECTA(l) 

= 

0.  ODO 

VECTA(2) 

= 

0.  ODO 

VECTA(3) 

= 

WZ2G 

VECTB(l) 

= 

WX2G 

VECTB(2) 

= 

WY2G 

VECTB(3) 

= 

O.ODO 

CALL  CPROD 

(VECTA,1 

/ECTB,MI1,MJ1,MK1) 

WDX2G 

= 

Mil 

WDY2G 

= 

MJ1 

WDZ2G 

= 

MK1 

TH3L 

= 

0.  ODO 

W3L 

= 

0.  ODO 

WD3L 

= 

0.  ODO 

TH3ANG 

= 

TH3L  * 

RADEG 

VX3G 

= 

WX2G 

WY3G 

= 

WY2G 

WZ3G 

= 

WZ2G 

WDX3G 

= 

WDX2G 

WDY3G 

= 

WDY2G 

WDZ3G 

= 

VDZ2G 

R2X 

= 

LNC0G2 

*  C0S(TH2L)  *  COS(THIL) 

R2Y 

= 

LNC0G2 

*  C0S(TH2L)  *  SIN(THIL) 

R2Z 

= 

LNC0G2 

*  SIN(TH2L) 

RJ2X 

= 

LINKL2 

*  C0S(TH2L)  *  COS(THIL) 

RJ2Y 

B 

LINKL2 

*  C0S(TH2L)  *  SIN(THIL) 

RJ2Z 

B 

LINKL2 

*  SIN(TH2L) 

R3X 

= 

RJ2X  + 

LNC0G3  *  COS(TH2L+TH3L)  *  COS(THIL) 

85 


* 


R3Y 

RJ2Y  +  LNCOG3  *  COS(TH2L+TH3L)  *  SIN(THIL) 

R3Z 

RJ2Z  +  LNCOG3  *  SIN(TH2L+TH3L) 

LTIPX 

RJ2X  +  LINKL3  *  COS(TH2L+TH3L)  *  COS(THIL) 

LTIPY 

RJ2Y  +  LINKL3  *  COS(TH2L+TH3L)  *  SIN(THIL) 

LTIPZ 

RJ2Z  +  LINKL3  *  SIN(TH2L+TH3L)  +  JZ1 

CALCULATE  THE 

GLOBAL  ACCELERATIONS 

AX1G 

0.  0D0 

AY1G 

0.  ODO 

AZ1G 

0.  ODO 

VECTA(l)  = 

WDX2G 

VECTA(2)  = 

WDY2G 

VECTA(3)  = 

WDZ2G 

VECTB(l)  = 

R2X 

VECTB(2)  = 

R2Y 

VECTB(3)  = 

R2Z 

CALL  CPROD 

(VECTA, VECTB, Mil, MJ1,MK1) 

VECTA(n  = 

WX2G 

VECTA(2)  = 

WY2G 

VECTA(3)  = 

WZ2G 

CALL  CPROD 

(VECTA, VECTB, MI 2, MJ2,MK2) 

VECTB(l)  = 

MI2 

VECTB(2)  = 

MJ2 

VECTB(3)  = 

MK2 

CALL  CPROD 

( VECTA , VECTB , MI 2 , MJ2 , MK2 ) 

AX2G 

Mil  +  MI2 

AY2G 

MJ1  +  MJ2 

AZ2G 

MK1  +  MK2 

VECTA(l)  as 

WDX3G 

VECTA(2)  = 

WDY3G 

VECTA(3)  = 

WDZ3G 

VECTB(l)  = 

R3X 

VECTB(2)  = 

R3Y 

VECTB(3)  = 

R3Z 

CALL  CPROD 

(VECTA, VECTB, MI 1,MJ1,MK1) 

VECTA(l)  = 

WX3G 

VECTA(2)  = 

WY3G 

VECTA(3)  = 

WZ3G 

CALL  CPROD 

(VECTA, VECTB, MI 2, MJ2,MK2) 

VECTB(l)  = 

MI2 

VECTB(2)  = 

MJ2 

VECTB(3)  = 

MK2 

CALL  CPROD 

(VECTA, VECTB, MI 2, MJ2,MK2) 

AX3G 

Mil  +  MI2 

AY3G 

MJ1  +  MJ2 

AZ3G 

MK1  +  MK2 

SET  MATRIX  D 

MATD(27)   = 

=  WDZ3G 

MATD(26)   = 

=  WDY3G 

MATD(25)   = 

:  WDX3G 

MATD(24)   = 

=  AZ3G 

MATD(23)   = 

=  AY3G 

MATD(22)   = 

=  AX3G 

MATD(21)   = 

=  -M3  *  AZ3G  -  WG3 

MATD(20)   = 

a  -M3  *  AY3G 

MATD(19)   = 

a  -M3  *  AX3G 

MATD(18)   = 

=  WDZ2G 
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MATD(17) 

= 

WDY2G 

MATD(16) 

= 

WDX2G 

MATD(15) 

s 

AZ2G 

MATD(14) 

= 

AY2G 

MATD(13) 

= 

AX2G 

MATD(12) 

= 

MATD(21) 

-  M2  *  AZ2G  -  WG2 

MATD(ll) 

= 

MATD(20) 

-  M2  *  AY2G 

MATD(IO) 

B! 

MATD(19) 

-  M2  *  AX2G 

MATD(9) 

= 

WDZ1G 

MATD(8) 

= 

WDY1G 

MATD(7) 

= 

WDX1G 

MATD(6) 

— 

AZ1G 

MATD(5) 

= 

AY1G 

MATD(4) 

= 

AX1G 

MATD(3) 

= 

MATD(12) 

-Ml  *  AZ1G   -  WG 

MATD(2) 

= 

MATD(ll) 

-  Ml  *  AY1G 

MATD(l) 

= 

MATD(IO) 

-  Ml  *  AX1G 

***  DIRECT  DYNAMICS 

PORTION  OF  PROGRAM  *** 

INPUT  JOINT 

EQUATIONS 

*      JOINT  ZERO 

EQUATIONS 

*       Wl  X  (Wl  X 

RB/G1) 

RBG1(1) 

=  JXO  - 

LCOGX(l) 

RBG1(2) 

=  JYO  - 

LCOGY(l) 

RBG1(3) 

=  JZO  - 

LCOGZ(l) 

VECTA(l)  =  Wl(l) 

VECTA(2)  =  Wl(2) 

VECTA(3)  =  Wl(3) 

* 


CALL  CPROD(VECTA,RBGl, MICO, MJCO, MKCO) 

VECTB(l)  =  MICO 

VECTB(2)  =  MJCO 

VECTB(3)  =  MKCO 
CALL  CPROD(VECTA,VECTB, MICO, MJCO, MKCO) 
Wl  X  (Wl  X  RA/G1) 

RAG1(1)  =  JX1  -  LCOGX(l) 

RAG1(2)  =  JY1  -  LCOGY(l) 

RAG1(3)  =  JZ1  -  LCOGZ(l) 

VECTA(l)  =  Wl(l) 

VECTA(2)  =  Wl(2) 

VECTA(3)  =  Wl(3) 
CALL  CPROD  (VECTA,RAG1,MIC1,MJC1,MKC1) 

VECTB(l)  =  MIC1 

VECTB(2)  =  MJC1 

VECTB(3)  =  MKC1 
CALL  CPROD  (VECTA,VECTB,MIC1,MJC1,MKC1) 
W2  X  (W2  X  RB/G2) 

RBG2(1)  =  JX1  -  LCOGX(2) 

RBG2(2)  =  JY1  -  LC0GY(2) 

RBG2(3)  =  JZ1  -  LCOGZ(2) 

VECTA(l)  =  W2(l) 

VECTA(2)  =  W2(2) 

VECTA(3)  =  W2(3) 
CALL  CPROD  (VECTA,RBG2,MIC2,MJC2,MKC2) 

VECTB(l)  =  MIC2 

VECTB(2)  =  MJC2 

VECTB(3)  =  MKC2 


87 


* 


* 


CALL  CPROD  (VECTA,VECTB,MIC2 

,MJC2,MKC2) 

W2  X  (W2  X  RA/G2) 

RAG2(1)  =  JX2  -  LCOGX(2) 

RAG2(2)  =  JY2  -  LCOGY(2) 

RAG2(3)  =  JZ2  -  LCOGZ(2) 

VECTA(l)  =  W2(l) 

VECTA(2)  =  W2(2) 

VECTA(3)  =  W2(3) 

CALL  CPROD  (VECTA,RAG2,MIC3, 

MJC3,MKC3) 

VECTB(l)  =  MIC3 

VECTB(2)  =  MJC3 

VECTB(3)  =  MKC3 

CALL  CPROD(VECTA,VECTB,MIC3, 

MJC3,MKC3) 

W3  X  (W3  X  RB/G3) 

RBG3(1)  =  JX2  -  LCOGX(3) 

RBG3(2)  =  JY2  -  LCOGY(3) 

RBG3(3)  =  JZ2  -  LCOGZ(3) 

VECTA(l)  =  W3(l) 

VECTA(2)  =  W3(2) 

VECTA(3)  =  W3(3) 

CALL  CPROD  (VECTA,RBG3,MIC4, 

MJC4,MKC4) 

VECTB(l)  =  MIC4 

VECTBt 2)  =  MJC4 

VECTBC3)  =  MKC4 

CALL  CPROD  (VECTA,VECTB,MIC4 

-,MJC4,MKC4) 

INERTIA  TRANSFORMATION 

TMAT(2,1)  =  -DCOS  (  THZ  ) 

TMAT(2,2)  =  -DSIN  (  THZ  ) 

TMAT(2,3)  =  0.  0D0 

DO  880  I  =  2,  3 

TMAT(3,1)  =  DRCSX(I) 

TMAT(3,2)  =  DRCSY(I) 

TMAT(3,3)  =  DRCSZ(I) 

VECTA(l)   =  TMAT(2,n 

VECTA(2)   =  TMAT(2,2) 

VECTA(3)   =  TMAT(2,3) 

VECTB(l)   =  TMAT(3,1) 

VECTB(2)   =  TMAT(3,2) 

VECTB(3)   =  TMAT(3,3) 

CALL  CPROD  (VECTA^'ECTB^ill, 

^JljMKl) 

TMAT(1,1)  =  Mil 

TMAT(1,2)  =  MJ1 

TMAT(1,3)  =  MK1 

TMATTR(1,1)  =  TMAT(1,1) 

TMATTR(1,2)  =  TMAT(2,1) 

TMATTR(1,3)  =  TMAT(3,1) 

TMATTR(2,1)  =  TMAT(1,2) 

TMATTR(2,2)  =  TMAT(2,2) 

TMATTR(2,3)  =  TMAT(3,2) 

TMATTR(3,1)  =  TMAT(1,3) 

TMATTR(3,2)  =  TMAT(2,3) 

TMATTR(3,3)  =  TMAT(3,3) 

DO  866  11  =  1,3 

DO  866  J  =1,3 

TEMP  =  0.  0D0 

DO  864  K  =1,3 
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TEMP  =  TMATTR(I1,K)  *  LIMAT(I,K,J) 

864 

CONTINUE 

MATTMP(I1,J)  =  TEMP 

866 

CONTINUE 

DO  874  11  =  1,3 

DO  874  J  =1,3 

TEMP  =  0.  0D0 

DO  872  K  =  1,3 

TEMP  =  MATTMP(I1,K)  *  TMAT(K,J)  + 

872 

CONTINUE 

NIMAT(I1,J)  =  TEMP 

874 

CONTINUE 

IXXT(I)  =  NIMAT(1,1) 

IXYT(I)  =  NIMAT(1,2) 

IXZT(I)  =  NIMAT(1,3) 

IYYT(I)  =  NIMAT(2,2) 

IYZT(I)  =  NIMAT(2,3) 

IZZT(I)  =  NIMAT(3,3) 

880 

CONTINUE 

5V 

ENTER  CONSTANS  INTO  MATRIX  A 

Vs-jV 

LINK  ONE 

* 

SUM  OF  FORCES 

* 

X  DIRECTION 

887 

MATA(1,1)   =   1.0D0 

MATA(1,4)   =  Ml 

MATA(1,10)  =  -1. 0D0 

* 

Y  DIRECTION 

MATA(2,2)   =   1.0D0 

MATA(2,5)   =  Ml 

MATA(2,11)  =  -1. 0D0 

* 

Z  DIRECTION 

MATA(3,3)   =   1.0D0 

MATA(3,6)   =  Ml 

MATA(3,12)  =  -1. 0D0 

* 

EQUATIONS  AT  JOINT  ZERO 

* 

X  DIRECTION 

MATA(4,4)  =   1.  ODO 

MATA(4,8)  =  RBG1(3) 

MATA(4,9)  =  -RBG1(2) 

* 

Y  DIRECTION 

MATA(5,5)  =   l.ODO 

MATA(5,7)  =  -RBG1(3) 

MATA(5,9)  =  RBGl(l) 

* 

Z  DIRECTION 

MATA(6,6)  =   l.ODO 

MATA(6,7)  =  RBG1(2) 

MATA(6,8)  =  -RBGl(l) 

* 

SUM  OF  MOMENTS  EQUATIONS 

X  DIRECTION 

MATA(7,2)   =  RBG1(3) 

MATA(7,3)   =  -RBG1(2) 

MATA(7,7)   =  -IXXT(l) 

MATA(7,8)   =  IXYT(l) 

MATA(7,9)   =   IXZT(l) 

MATA(7,11)  =  -RAG1(3) 

+  TEMP 


TEMP 
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MATA(7,12)  = 

RAG1(2) 

* 

Y  DIRECTION 

MATA(8,1)   =  • 

-RBG1(3) 

MATA(8,3)   = 

RBGl(l) 

MATA(8,7)   = 

IXYT(l) 

MATA(8,8)   =  • 

-IYYT(l) 

MATA(8,9)   = 

IYZT(l) 

MATA(8,10)  = 

RAG1(3) 

MATA(8,12)  =  • 

-RAGl(l) 

* 

Z  DIRECTION 

MATA(9,1)   = 

RBG1(2) 

MATA(9,2)   =  • 

-RBG1(1) 

MATA(9,7)   = 

IXZT(l) 

MATA(9,8)   = 

IYZT(l) 

MATA(9,9)   = 

-IZZT(l) 

MATA(9,10)  = 

-RAG1(2) 

MATA(9,11)  = 

RAG1(1) 

** 

LINK  TWO 

* 

SUM  OF  FORCES 

* 

X  DIRECTION 

939 

MATA(10,10)  = 

1.  0D0 

MATA(10,13)  = 

M2 

MATA(10,19)  = 

-1.  ODO 

* 

Y  DIRECTION 

MATA(11,11)  = 

1.  ODO 

MATA( 11,14)  = 

M2 

MATA( 11,20)  = 

-1.  ODO 

* 

Z  DIRECTION 

MATA(12,12)  = 

l.ODO 

MATA(12,15)  = 

M2 

MATA( 12,21)  = 

-1.  ODO 

* 

EQUATIONS  AT  JOINT  ONE 

* 

X  DIRECTION 

MATA(13,4)   = 

-1.  ODO 

MATA(13,8)   = 

-RAG1(3) 

MATA(13,9)   = 

RAG1(2) 

MATA(13,13)  = 

1.  ODO 

MATA(13,17)  = 

RBG2(3) 

MATA(13,18)  = 

-RBG2(2) 

* 

Y  DIRECTION 

MATA(14,5)   = 

-1.  ODO 

MATA(14,7)   = 

RAG1(3) 

MATA(14,9)   = 

-RAG1(1) 

MATA(14,14>)  = 

1.  ODO 

MATA(14,16;  = 

-RBG2(3) 

MATA(14,18)  = 

RBG2(1) 

* 

Z  DIRECTION 

MATA(15,6)  --= 

-l.ODO 

MATA(15,7)   = 

-RAG1(2) 

MATA(15,8)   = 

RAG1(1) 

MATA( 15,15)  = 

l.ODO 

MATA(15,16)  = 

RBG2(2) 

MATA(15,17)  = 

-RBG2(1) 

* 

SUM  OF  MOMENTS 

EQUATIONS 

* 

X  DIRECTION 

MATA(16,11)  = 

RBG2(3) 
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MATA(16,12) 

= 

-RBG2(2) 

MATA( 16,16) 

= 

-IXXT(2) 

MATA(16,17) 

= 

IXYT(2) 

MATA(16,18) 

= 

IXZT(2) 

MATA( 16,20) 

= 

-RAG2(3) 

MATA( 16,21) 

= 

RAG2(2) 

*      Y  DIRECTION 

MATA(17,10) 

= 

-RBG2(3) 

MATA(17,12) 

= 

RBG2(1) 

MATA(17,16) 

= 

IXYT(2) 

MATA(17,17) 

= 

-IYYT(2) 

MATA(17,18) 

= 

IYZT(2) 

MATA(17,19) 

= 

RAG2(3) 

MATA( 17,21) 

= 

-RAG2(1) 

*      Z  DIRECTION 

MATA( 18,10) 

= 

RBG2(2) 

MATA(18,11) 

= 

-RBG2(1) 

MATA(18,16) 

= 

IXZT(2) 

MATA(18,17) 

= 

IYZT(2) 

MATA(18,18) 

= 

-IZZT(2) 

MATA(18,19) 

= 

-RAG2(2) 

MATA( 18,20) 

= 

RAG2(1) 

**     LINK  THREE 

SUM  OF  FORCES 

*      X  DIRECTION 

1000     MATA(19,19) 

= 

1.  0D0 

MATA( 19,22) 

= 

M3 

*      Y  DIRECTION 

MATA( 20,20) 

= 

l.ODO 

MATA(20,23) 

= 

M3 

*      Z  DIRECTION 

MATA(21,21) 

= 

1.  ODO 

MATA(21,24) 

s 

M3 

*      EQUATIONS  AT  JOINT  TWO 

*      X  DIRECTION 

MATA(22,13) 

= 

-1.  ODO 

MATA(22,17) 

= 

-RAG2(3) 

MATA(22,18) 

= 

RAG2(2) 

MATA(22,22) 

= 

1.  ODO 

MATA(22,26) 

= 

RBG3(3) 

MATA(22,27) 

s 

-RBG3(2) 

*      Y  DIRECTION 

MATA(23,14) 

= 

-1.  ODO 

MATA(23,16) 

B 

RAG2(3) 

MATA(23,18) 

S 

-RAG2(1) 

MATA(23,23) 

= 

1.  ODO 

MATA(23,25) 

S 

-RBG3(3) 

MATA(23,27) 

= 

RBG3(1) 

*      Z  DIRECTION 

MATA(24,15) 

= 

-1.  ODO 

MATA(24,16) 

= 

-RAG2(2) 

MATA(24,17) 

= 

RAG2(1) 

MATA( 24,24^ 

= 

1.  ODO 

MATA( 24,25) 

= 

RBG3(2) 
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MATA(24,26)  = 

-RBG3(1) 

* 

SUM  OF  MOMENTS  EQUATIONS 

* 

X  DIRECTION 

MATA(25,20)  = 

RBG3(3) 

MATA(25,21)  = 

-RBG3(2) 

MATA(25,25)  = 

-IXXT(3) 

MATA(25,26)  = 

IXYT(3) 

MATA(25,27)  = 

IXZT(3) 

* 

Y  DIRECTION 

MATA(26,19)  = 

-RBG3(3) 

MATA(26,21)  = 

RBG3(1) 

MATA(26,25)  = 

IXYT(3) 

MATA(26,26)  = 

-IYYT(3) 

MATA(26,27)  = 

IYZT(3) 

* 

Z  DIRECTION 

MATA(27,19)  = 

RBG3(2) 

MATA(27,20)  = 

-RBG3(1) 

MATA(27,25)  = 

IXZT(3) 

MATA(27,26)  = 

IYZT(3) 

MATA(27,27)  = 

-IZZT(3) 

***** 

FIRST  LINK  IS  CONSTRAINED  TO  ROTATE  IN  Z  DIRECTION   ***** 

1051 

DO  1056  I  =  45 

8 

DO  1055  J  = 

1,27 

MATA(I,J) 

=  0.  0D0 

* 

MATA(J,I) 

=  0.  0D0 

1055 

CONTINUE 

1056 

CONTINUE 

MATA(4,4) 

=  1.  0D0 

MAT A (5, 5) 

=  1.  0D0 

MATA(6,6) 

=  1.  0D0 

MATA(7,7) 

=  1.  0D0 

MATA(8,8) 

=  1.  0D0 

DO  1065  J  =  1 

27 

MATA(18,J) 

=  0.  0D0 

MATA(27,J) 

=  0.  0D0 

1065 

CONTINUE 

IZZT(2)  =  IZZT(2)  +  M2  *  (DSQRT(LCOGX(2)**2+LCOGY(2)**2) )**2 

IZZT(3)  =  IZZT(3)  +  M3  *  (DSQRT(LCOGX(3)**2+LCOGY(3)**2)  )**  2 

MATA(9,9) 

=  -(IZZT(1)+IZZT(2)+IZZT(3)) 

MATA(18,9) 

=  -1.  0D0 

MATA(18,18) 

=   1.  0D0 

MATA(27,9) 

=  -1.  0D0 

MATA(27,27) 

=   1.0D0 

*  MULT 

DO  758  J  =  1,27 

SUMl  =  0. 

0D0 

DO  755  K 

=  1,27 

SUMl  =  SUMl  +  MATA(J,K)  *  MATD(K) 

755 

CONTINUE 

MATC(J)  = 

=  SUMl 

758 

CONTINUE 

T2Z  =   -MATC(27) 

T2Y  =   -MATC(26) 

T2X  =   -MATC(25) 

T1Z  =   T2Z  ■ 

•  MATC(18) 

T1Y  =   T2Y  ■ 

■  MATC(17) 
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T1X  = 

T2X 

-  MATC(16) 

TOZ   = 

TIZ 

-  MATC(9) 

TOY  = 

TIY 

-  MATC(8) 

TOX  = 

T1X 

-  MATC(7) 

*    ENTER  KNOWNS 

INTO 

MATB 

1077     MATB(l) 

= 

O.ODO 

MATB(2) 

= 

0.  ODO 

MATB(3) 

= 

-WG1 

MATB(4) 

sb 

AXO  -  MICO 

MATB(5) 

BS 

AYO  -  MJCO 

MATB(6) 

= 

AZO  -  MKCO 

MATB(7) 

= 

T1X  -  TOX 

MATB(8) 

= 

TIY  -  TOY 

MATB(9) 

= 

TIZ  -  TOZ 

MATB(IO) 

BS 

0.  ODO 

MATB(ll) 

bs 

0.  ODO 

MATB(12) 

= 

-WG2 

MATB(13) 

S5 

MIC1  -  MIC2 

MATB(14) 

SB 

MJC1  -  MJC2 

MATB(15) 

= 

MKC1  -  MKC2 

MATB(16) 

= 

T2X  -  T1X 

MATB(17) 

= 

T2Y  -  TIY 

MATB(18) 

B! 

T2Z  -  TIZ 

MATB(19) 

= 

0.  ODO 

MATB(20) 

SB 

0.  ODO 

MATB(21) 

= 

-WG3 

MATB(22) 

= 

MIC3  -  MIC4 

MATB(23) 

= 

MJC3  -  MJC4 

MATB(24) 

= 

MKC3  -  MKC4 

MATB(25) 

= 

-T2X 

MATB(26) 

= 

-T2Y 

MATB(27) 

= 

-T2Z 

*****   FIRST  LINK 

:  is 

CONSTRAINED  TO  ROTATE  IN  Z  DIRECTION 

***** 

DO  1107  1 

:  =  4 

,8 

MATB(I) 

= 

0.  ODO 

1107     CONTINUE 

MATB(18)   = 

O.ODO 

MATB(27)   = 

0.  ODO 

1112 


1117 


1120 


1123 
1124 

1126 
1127 


CALL  EQUATION  SOLVER  PROGRAM  FROM  IMSL 

CALL  LEQT2F(MATA,M,N,IA,MATB)IDGT,WKAREA,IER) 

IF  (IER.  EQ.  0)  THEN 

GOTO  1133 

ELSE 

WRITE  (*,1117)  IER 

FORMAT  (15) 

DO  1127   I  =  1,  27 

WRITE  (*,1120)  I 

FORMAT  (17) 

DO  1124   J  =  1,  27,  3 

WRITE  (*,1123)  J,MATA(I,J),J+l,MATA(I,J+l),J+2,MATA(I,J+2) 

FORMAT  (I5,F13.5,I5,F13.5,I5,F13.5) 

CONTINUE 

WRITE  (*,1126)  I,MATB(I) 

FORMAT  (I3,F15. 5) 

CONTINUE 
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* 

1133 


■>V 


CALL  ENDJOB 
END  IF 
FIND  LCOGX,LCOGY,LCOGZ,THETA  VALUE S,WX,WY,WZ 
JOINT  ZERO 

FXO  =  MATB(l) 
FYO  =  MATB(2) 
FZO  =  MATB(3) 
LINK  ONE 


rt 

SINCE  LINK1 

IS  CONSTRAIN  TO  ROTATE  AROUND  THE  Z  ONLY 

AX1  =  0. 

0D0 

AY1  =  0. 

ODO 

AZl  =  0. 

ODO 

*141 

AX1 

=  MATB(4) 

* 

VELX1 

=  INTGRL(0. ,AX1) 

* 

LC0GX1 

=  INTGRL(X1,VELX1) 

* 

LCOGX(l) 

=  LC0GX1 

* 

AY1 

=  MATB(5) 

* 

VELY1 

=  INTGRL(0.  ,AY1) 

■jV 

LC0GY1 

=  INTGRL(Y1,VELY1) 

i- 

LCOGY(l) 

=  LC0GY1 

•it 

AZl 

=  MATB(6) 

•jV 

VELZ1 

=  INTGRL(0.  ,AZ1) 

* 

LC0GZ1 

=  INTGRL(Z1,VELZ1) 

* 

LCOGZ(l) 

=  LC0GZ1 

WD  IX 

=  MATB(7) 

W1X 

=  INTGRL(0.  ,WD1X) 

W'DX(l) 

=  WD  IX 

Wl(l) 

=  W1X 

WDIY 

=  MATB(8) 

W1Y 

=  INTGRL(0.  ,WD1Y) 

WDY(l) 

=  WDIY 

Wl(2) 

=  W1Y 

WD1Z 

=  MATB(9) 

WIZ 

=  INTGRL(2.  ,WD1Z) 

WDZ(l) 

=  WD1Z 

Wl(3) 

=  WIZ 

1166 

THZ 

=  INTGRL(0.  ,W1Z) 

THZANG 

=  THZ  *  RADEG 

COSTHZ 

=  DCOS(THZ) 

SINTHZ 

=  DSIN(THZ) 

*   IF 

THE  1ST  LINK  IS  CONSTRAIN  TO  ROTATE  IN  THE  Z  DIRECTION  ONLY 

THE  DIRECTION 

COSINE  AND  DIRECTION  COSINE  ANGLES  ARE  CONSTANT 

AND  DO  NOT  NEED  TO  BE  CALCULATED 

JL 

JOINT  ONE 

1174 

FX1  =  MATB(IO) 

FY1  =  MATB(ll) 

FZ1  =  MATB(12) 

** 

LINK  TWO 

1178 

AX2 

=  MATB(13) 

VELX2 

=  INTGRL(-0.416,AX2) 

LCOGX2 

=  INTGRL(X2,VELX2) 

LCOGX(2) 

=  LC0GX2 

AY2 

=  MATB(14) 

VELY2 

=  INTGRL(0.  ,AY2) 

LCOGY2 

=  INTGRL(Y2,VELY2) 

LCOGY(2) 

=  LC0GY2 
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AZ2 

= 

MATB(15) 

VELZ2 

= 

INTGRL(0. 416, AZ2) 

LC0GZ2 

= 

INTGRL(Z2,VELZ2) 

LC0GZ(2) 

= 

LCOGZ2 

WD2X 

= 

MATB(16) 

W2X 

= 

INTGRL(2.  ,WD2X) 

WDX(2) 

= 

WD2X 

W2(l) 

= 

W2X 

WD2Y 

as 

MATB(17) 

W2Y 

= 

INTGRL(0.  ,WD2Y) 

WDY(2) 

sa 

WD2Y 

W2(2) 

= 

W2Y 

WD2Z 

= 

MATB(18) 

W2Z 

= 

INTGRL(2.  ,WD2Z) 

W2(3) 

— 

W2Z 

WDZ(2) 

= 

WD2Z 

GET  THE  DIRECTION  COSINES  FOR  T 

THE  LINK  TWO 
DRCSX(2)   =  (LCOGX2  -  JX1)  /  LNCOG2 
DRCSY(2)   =  (LCOGY2  -  JY1)  /  LNCOG2 
DRCSZ(2)   =  (LC0GZ2  -  JZ1)  /  LNCOG2 
AMP  =  DSQRT(DRCSX(2)*DRCSX(2)+DRCSY(2)*DRCSY(2)+. 

DRCSZ(2)*DRCSZ(2)) 
DRCSX(2)  =  DRCSX(2)/AMP 
DRCSY(2)  =  DRCSY(2)/AMP 
DRCSZ(2)  =  DRCSZ(2)/AMP 
DRCANX(2)  =  DACOS(DRCSX(2)) 

DAC0S(DRCSY(2)) 

DACOS(DRCSZ(2)) 


DRCANY(2)  = 
DRCANZ(2)  = 
*  JOINT  LOCATION 
1215  JX2  =  JX1  + 
JY2  =  JY1  + 
JZ2  =  JZ1  + 


RADEG 
RADEG 
RADEG 


LINKL2 
LINKL2 
LINKL2 


*  DRCSX(2) 

*  DRCSY(2) 

*  DRCSZ(2) 


1220 


1224 


JOINT  TWO 

FX2  =  MATB 
FY 2  =  MATB 
FZ2  =  MATB 
LINK  THREE 
AX  3 
VELX3 
LC0GX3 
LC0GX(3) 
AY3 
VELY3 
LC0GY3 
LC0GY(3) 
AZ3 
VELZ3 
LC0GZ3 
LC0GZ(3) 
WD3X 
W3X 
WDX(3) 
W3(l) 
WD3Y 
W3Y 


(19) 
(20) 
(21) 

=  MATB(22) 

=  INTGRL(-1.282,AX3) 

=  INTGRL(X3,VELX3) 

=  LC0GX3 

=  MATB(23) 

=  INTGRL(0.  ,AY3) 

=  INTGRL(Y3,VELY3) 

=  LC0GY3 

=  MATB (24) 

=  INTGRL(1.  282, AZ3) 

=  INTGRL(Z3,VELZ3) 

=  LC0GZ3 

=  MATB (25) 

=  INTGRL(2.  ,WD3X) 

=  WD3X 

=  W3X 

=  MATB (26) 

=  INTGRL(0.  ,WD3Y) 
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*  CALC 
1249 


WDY(3) 
W3(2) 
WD3Z 
W3Z 
WDZ(3) 
W3(3) 
DIRECTIONAL 
DRCSX(3)  = 
DRCSY(3)  = 
DRCSZ(3)  = 
AMP 


TIP 


1261 


1265 


WD3Y 
W3Y 

MATB(27) 
INTGRL(2.  ,WD3Z) 
WD3Z 
W3Z 
COSINES  FOR  LINK  THREE 
(LCOGX3  -  JX2)  /  LNCOG3 
(LCOGY3  -  JY2)  /  LNCOG3 
(LCOGZ3  -  JZ2)  /  LNCOG3 
=  DSQRT(DRCSX(3)*DRCSX(3)+DRCSY(3)*DRCSY(3)+. 
DRCSZ(3)*DRCSZ(3)) 
DRCSX(3)  =  DRCSX(3)/AMP 
DRCSY(3)  =  DRCSY(3)/AMP 
DRCSZ(3)  =  DRCSZ(3)/AMP 
DRCANX(3)  =  DACOS(DRCSX(3)) 
=  DACOS(DRCSY(3)) 
=  DAC0S(DRCSZ(3)) 


DRCANY(3) 
DRCANZ(3) 
LOCATION 
TIPX  =  JX2  +  LINKL3  *  DRCSX(3) 
TIPY  =  JY2  +  LINKL3  *  DRCSY(3) 
TIPZ  =  JZ2  +  LINKL3  *  DRCSZ(3) 
FIND  THE  ANGLE  BETWEEN  THE  LIMKS 


RADEG 
RADEG 
RADEG 


ANG23X 

ANG23Y 

ANG23Z 

ANG12X 

ANG12Y 

ANG12Z 

ANG12 

ANG23 


DRCANX(2) 
DRCANY(2) 
DPCANZ(2) 
DRCANX(l) 
DRCANY(l) 
DRCANZ(l) 
DRCANZ(2) 
ANG23X  + 


-  DRCANX(3) 

-  DRCANY(3) 

-  DRCANZ(3) 

-  DRCANX(2) 

-  DRCANY(2) 

-  DRCANZ(2) 

-  DRCANZ(l) 
ANG23Y  +  ANG23Z 


■>v 


CLACULATE  THE  ERROR  BETWEEN  KNOWN  TIP  POSITION  AND  CALCULATED 
ERROR1  =  ABS  (  LTIPX  -  TIPX  )  /  0. 867  *  100. 0 
ERR0R2  =  ABS  (  LTIPY  -  TIPY  )  /  0. 867  *  100. 0 
ERROR3  =  ABS  (  LTIPZ  -  TIPZ  )  /  0. 867  *  100. 0 


END 
STOP 

FORTRAN 

*     SUBROUTINE  TO  COMPUTE  THE  CROSS  PRODUCT  OF  TWO  VECTORS 


SUBROUTINE  CPROD( VECTA,VECTB ,MI ,MJ,MK) 
1284         IMPLICIT  REAL*8  (A-Z) 

DIMENSION  VECTA(3),VECTB(3) 
MI  =  VECTA(2)  *  VECTB(3)  -  VECTA(3) 
MJ  =  VECTA(3)  *  VECTB(l)  -  VECTA(l) 
MK  =  VECTA(l)  *  VECTB(2)  -  VECTA(2) 
RETURN 
END 


* 


VECTB(2) 
VECTB(3) 
VECTB(l) 
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APPENDIX  D 
ROBOT  VALIDATION  PROGRAM 


TITLE  ROBOT  TORQUE  VALIDATION  PROGRAM 

************************************************************************* 

*  THIS   IS  A  PROGRAM  THAT  USES  ACTUAL  DATA  FROM  THE  NEPTUNE   II   ROBOT  TO  * 

*  OBTAIN  TORQUES   TO    INPUT   INTO  THE   NONSINGULAR   SIMULATION   OF   A   3   LINK     * 

*  RIGID  MANIPULATOR.      THE   ORIGINAL  PROGRAM  WAS  WRITTEN  BY  LT  ALTINOK       * 

*  AND  HAS   BEEN  GREATLY  MODIFIED  TO   INCLUDE   3  D  MOTION  AND  GRAVITATIONAL* 

*  EFFECTS   BY     LT     R.    M.      VERBOS        USN  SEPTEMBER   1988  * 

•i— *J—  -wl^  fc1*  »'*  ■*'-   ^'-   »'»   lAtflMfl  JdJd  ^.*^*Jf*  *'*  |Jfl  kJU  *T-   *J-    -"w  fc."^*.*-  *.'*%-"—  *J—   *.T*   ^J»   .-',*■* I*   fcJ*   ^'*  J*«'#ut'4J#J'-,'kV*'*hf«k'*<VhVw't-''fe''*-*h''-*^«h''h'"k''k1'k^k<*k'**''>J'^k''a^*^*^*feVh'**<*k'Jk''^'b-''^« 

«»  TV  H  *»  /*  *»  /v  »v  #»  *\  ft  <»  *»  »s  n  ««  t\  f\  0%  >t  *C7V  »w%  *»  *C#»  /iTTit  SS  M  /tTt  #*  «*  *v  it  **#*  /*  n  *V  .  ■.   -v   *  ■  **  f*  ft  *»  7*  #s»*  *%  **  7t   /*  »*  *»  #*  #*  #t  7X  f%  #C***c*»  /»  **  **  Ts  n 

TERMINAL    . 

METHOD  RKSFX 

PRINT   . 10,DRCANZ(3) 

CONTROL  FINTIM  =7.0,    DELT  =   .01,    DELMAX  =   .1,    DELPRT  =  .10 

SAVE   .10,    DRCANZ(3) 

GRAPH   (DE=TEK618)   TIME ,DRCANZ( 3) 

D  DIMENSION  MATA(27 ,27) .MASS( 3 ,2) ,L( 3 ,2) ,RX( 3,2) ,RY( 3 ,2) ,RZ(3 ,2) 

D  DIMENSION   IXX( 3 ,2) , IXZ(3 ,2) , IXY( 3 ,2) , IYY( 3,2) , IYZ( 3 ,2) , IZZ(  3,2) 

D  DIMENSION   IMAT( 3 ,3 ,3) ,NIMAT( 3,3) ,TMAT( 3 ,3) ,TMATTR( 3,3) ,MATTMP( 3 ,3) 

D  DIMENSION   LIMAT(3,3,3) 

FIXED        IER,I,J,M,K,P,N,IA,IDGT,A,I1 

ARRAY   MATB(27),LC0GX(3),LC0GY(3),LC0GZ(3) 

ARRAY  VECTA0(3),VECTB0(3),VECTA1(3),VECTB1(3),VECTA2(3),VECTB2(3) 

ARRAY  VECTA(3),VECTB(3) 

ARRAY  WDX(3),WDY(3),WDZ(3),W1(3),W2(3),W3(3) 

ARRAY  RBG1(3) ,RAG1( i) ,RBG2( 3) ,RAG2(3) ,RBG3(3) 

ARRAY  WKAREA(850) 

ARRAY   IXXT( 3) , IYYT( 3) , IZZT( 3) , IXYT( 3) , IXZT( 3) , IYZT( 3) 

ARRAY  DRCANX(3) ,DRCANY(3) ,DRCANZ(3) 

ARRAY  DRCSX(3),DRCSY(3),DRCSZ(3) 


INITIAL 

if****        INPUT        ***** 

*  INPUT  PARAMETER  CONSTANTS 

IDGT  =  6 

G  =  9.81D0 

N  =  27  » 

M  =   1 

IA  =  27 

IER  =  0 

LEVELQ  =  0 

LEVLDQ  =  0 

*  INPUT  DISTANCE  FROM  COG  OF  LINK  TO  CENTER  OF  MASS 

*  FOR  EACH  LINK  ENDS 
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L(l,l) 

=  0. 

1953D0 

L(l,2) 

=  0. 

065 1D0 

L(2,l) 

=  0. 

208D0 

L(2,2) 

=  0. 

208D0 

L(3,l) 

=  0. 

1746D0 

L(3,2) 

=  0. 

1746D0 

* 


*  INPUT  THE  LINK  LENGTHS  OF  THE  ROBOT 

LINKL1  =  0. 2604D0 
LINKL2  =  0. 416D0 
LINKL3  =  0. 3498D0 

*  INPUT  JOINT  LOCATIONS  IN  METERS 

JXO  =  0.  0D0 

JYO  =  0.  ODO 

JZO  =  0.  ODO 

JX1  =  0.  ODO 

JY1  =  0.  ODO 

JZ1  =  0. 2604D0 

JX2  =  0.  ODO 

JY2  =  0.416D0 

JZ2  =  0. 2604D0 

INPUT  TORQUE  CONSTANTS 

TOX  =  0.  ODO 

TOY  =  0. ODO 

TOZ  =  0. ODO 

T1X  =  0.  ODO 

T1Y  =  O.ODO 

T1Z  =  0.  ODO 

T2X  =  0. ODO 

T2Y  =  0.  ODO 

T2Z  =  0. ODO 

TGI  =  0.  ODO 

TG2  =  0.  ODO 

T1FNC  =  0. ODO 

T2FNC  =  0. ODO 

INPUT  MASS  AT  LINK  ENDS  IN  KILOGRAMS 
110  MASS(1,1)  =  2. 273D0 
MASS(1,2)  =  6. 818D0 
MASS(2,1)  =  0.455D0 
MASS(2,2)  =  0.455D0 
MASS(3,1)  =  5. 909D0 
MASS(3,2)  =  5.909D0 

*  INPUT  LOCATION  OF  LINK  CENTERS  OF  GRAVITY 
120        LCOGX(l)  =  O.ODO 

XI       =  LCOGX(l) 

LCOGY(l)  =  0. ODO 

Yl       =  LCOGY(l) 

LCOGZ(l)  =  0. 10D0 

Zl       =  LCOGZ(l) 

LC0GX(2)  =  O.ODO 

X2       =  LC0GX(2) 

LC0GY(2)  =  0. 208D0 
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Y2 

= 

LC0GY(2) 

LC0GZ(2) 

= 

0. 2604D0 

Z2 

= 

LC0GZ(2) 

LC0GX(3) 

= 

0.  0D0 

X3 

S5 

LCOGX(3) 

LC0GY(3) 

= 

0.4159D0 

Y3 

rr 

LCOGY(3) 

LC0GZ(3) 

= 

0. 0858D0 

Z3 

= 

LCOGZ(3) 

*  INPUT  MASS  OF  EACH  LINK  IN  KG 

Ml  =  9. 091D0 
M2  =  0. 910D0 
M3  =  11.818D0 

*  INPUT  ACCELERATIONS  OF  JOINT  ZERO 

AXO  =  0.  ODO 
AYO  =  0. ODO 
AZO  =  0.  ODO 

*  INPUT  THE  INITIAL  DIRECTION  COSINES 

DRCSX(l)  =  O.ODO 

DRCSY(l)  =  0.  ODO 

DRCSZ(l)  =  1.  ODO 

DRCSX(2)  =  0.  ODO 

DRCSY(2)  =  1.  ODO 

DRCSZ(2)  =  0.  ODO 

DRCSX(3)  =  0.  ODO 

DRCSY(3)  =  0.  ODO 

DRCSZ(3)  =  -l.ODO 

*  INPUT  THE  INITIAL  DIRECTION  COSINE  ANGLES 

DRCANX(l)  =  90. ODO 

DRCANY(l)  =  90. ODO 

DRCANZ(l)  =  0.  ODO 

DRCANX(2)  =  90. ODO 

DRCANY(2)  =  0.  ODO 

DRCANZ(2)  =  90. ODO 

DRCANX(3)  =  90. ODO 

DRCANY(3)  =  90. ODO 

DRCANZ(3)  =  180. ODO 

*****   INITIALIZE   ***** 

*  OMEGA  AND  OMEGA  DOT 
160       DO  170  I  =  1,3 

W1(I)  =  O.ODO 

W2(I)  =  O.ODO 

W3(I)  =  O.ODO 

WDX(I)  =  0.  ODO 

WDY(I)  =  0.  ODO 

WDZ(I)  =  O.ODO 
170       CONTINUE 

W3(l)  =  0.  158D0 

W3IC  =  W3(l) 

THZ  =  0.  ODO 
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175 
180 


INITIALIZE  MATRIX  A  AND  B  TO  ZERO 
DO  180  I  =  1,27 
DO  175  J  =  1,27 

MATA(I.J)  =  0.0D0 
CONTINUE 
MATB(I)  =  0. 0D0 
CONTINUE 


CONSTANT  TO  CONVERT  STRIP  DATA  TO  DELTA  P 

RDVTOV  =  0. 2D0 

RAREA  =  0. 00312D0 

RLA  =  0.  0597/2.0 

RVTOP  =  10. ODO 

RLBTON  =  4. 448D0 

RINTOM  =  39. 37D0 

RCNFAC  =  RDVTOV*RVTOP*RAREA*RLA*RLBTON*RINTOM**2 


***** 


CALCULATIONS 


«»  ft  7%   *v  /« 


*      WEIGHTS  (NEWTONS) 
185        WG1  =  M1*G 
WG2  =  M2*G 

WG3  =  M3*G 


COMPUTE  THE  LENGTH  FROM  THE  INBOARD  JOINT  TO  COG 


LNCOG1  =  DSQRT  ( 


LX2  = 
LY2  = 
LZ2  = 
LNCOG 
LX3  = 
LY3  = 
LZ3  = 
LNCOG 
V3IC 


LCOGX(2) 
LCOGY(2) 
LCOGZ(2) 

2  =  DSQRT 
LCOGX(3) 
LCOGY(3) 
LCOGZC3) 

3  =  DSQRT 
=  LNCOG 3  i 


LCOGX(l)*LCOGX(l) 
LCOGZ(l)*LCOGZ(l) 
JX1 
JY1 
JZ1 

LX2*LX2  +  LY2-LY2 
JX2 
JY2 
JZ2 

LX3*LX3  +  LY3-LY3 
W3(l) 


LCOGY(l)*LCOGY(l)  +. 


+  LZ2*LZ2  ) 


+  LZ3*LZ3  ) 


L(3,2) 
L(3,2)  = 


(13.5  *  RCNFAC)  /  (  MASS(3,2)  *  G)   -  LNCOG3 


COMPUTE  INITIAL  INERTIAS  BASED  ON  POINT  MASSES 
IN  GLOBAL  COORDINATES 


190 


DO 


200 


IXX(I,1) 

IXX(I,2) 

IXXT(I) 

IYY(I,1) 

IYY(I,2) 

IYYT(I) 


225  I  = 
RX(I,1)  = 
RX(I,2)  = 
RY(I,1)  = 
RY(I,2)  = 
RZ(I,1)  = 
RZ(I,2)  = 
=  MASS (I 


1,3 


-L(I,1)  * 

L(I,2)  * 

-L(I,1)  * 

L(I,2)  * 

-L(I,1)  * 

L(I,2)  * 

1)  *  ((RY( 


MASS(I,2) 


((RY(I,2) 
=  IXX(I,1)  +  IXX(I,2) 
=  MASS(I,1)  *  ((RX(I,1) 
=  MASS(I,2)  *  ((RX(I,2) 
=  IYY(I,1)  +  IYY(I,2) 


DRCSX(I) 
DRCSX(I) 
DRCSY(I) 
DRCSY(I) 
DRCSZ(I) 
DRCSZ(I) 
1,1)  *  RY(I,1)) 
RY(I,2)) 


RX(I,1)) 
RX(I,2)) 


+ 
+ 

+ 
+ 


(RZ(I,1)  *  RZ(I,1))) 
(RZ(I,2)  *  RZ(I,2))) 


(RZ(I,1) 

(RZ(I,2) 


* 
* 


RZ(I,1))) 
RZ(I,2))) 
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=  IYZ(I,1)  + 


.LE. 
10 


IZZ(I,1) 

IZZ(I,2) 

IZZT(I) 
205    IXY(I,1) 

IXY(I,2) 

IXYT(I) 

IXZ(I,1) 

IXZ(I,2) 

IXZT(I) 

IYZ(I,1) 

IYZ(I,2) 

IYZT(I) 

IF  (IXXT(I) 

IXXT(I)   = 

ELSE 

IXXT(I)  =  IXXT(I) 

END  IF 

IF  (IYYT(I)  .LE.  . 

IYYT(I)   =  .01 

ELSE 

IYYT(I)   =  IYYT(I) 

END  IF 

IF  (IZZT(I)  .LE.  . 

IZZT(I)   =  .  01 

ELSE 

IZZT(I)   =  IZZT(I) 

END  IF 

IMAT(I,1,1)  = 
IMAT(I,1,2)  = 
IMAT(I,1,3)  = 
IMAT(I,2,1)  = 
IMAT(I,2,2)  = 
IMAT(I,2,3)  = 
IMAT(I,3,1)  = 
IMAT(I,3,2)  = 
IMAT(I,3,3)  = 
225        CONTINUE 


MASS(I,1)  *  ((RX(I,1) 
MASS(I,2)  *  ((RX(I,2) 
IZZ(I,1)  +  IZZ(I,2) 
MASS(I.l)  *  RX(I,1)  * 
MASS(I,2)  *  RX(I,2) 
IXY(I,1)  +  IXY(I,2) 
MASS(I,1)  *  RZ(I,1) 
MASS(I,2)  *  RZ(I,2) 
IXZ(I,1)  +  IXZ(I,2) 
MASS(I,1)  *  RY(I,1) 
MASS(I,2)  *  RY(I,2) 


10) 


IYZ(I,2) 
THEN 


01)  THEN 


01)  THEN 


IXXT(I) 

IXYT(I) 

IXZT(I) 

-IXYT(I) 

IYYT(I) 

IYZT(I) 

-IXZT(I) 

-IYZT(I) 

IZZT(I) 


RX(I,1)) 
RX(I,2)) 


(RY(I,1) 
(RY(I,2) 


RY(I,1))) 
RY(I,2))) 


RY(I,1) 
RY(IS2) 

RX(I,1) 
RX(I,2) 

RZ(I,1) 
RZ(I,2) 


*  DUE  TO  LINK 

IXXT(l)  = 

IXYT(l)  = 

IXZT(l)  = 

IYYT(l)  = 

IYZT(l)  = 

IZZT(l)  = 


1  CONSTRAINTS 
IMAT( 1,1,1) 
IMAT(1,1,2) 
IMAT( 1,1,3) 
IMAT( 1,2,2) 
IMAT(1,2,3) 
IMAT( 1,3,3) 


LINK  1  INERTIAS  ARE  CONSTANT 


TRANSFORM  THE  INITIAL  INERTIAS  TO  LOCAL  COORDINATED 


DO  9  1=2, 
TMAT(2,1) 
TMAT(2,2) 
TMAT(2,3) 
TMAT(3,1) 
TMAT(3,2) 
TMAT(3,3) 


-DCOS  ( 
-DSIN  ( 
0.  ODO 
DRCSX(I) 
DRCSY(I) 
DRCSZ(I) 


THZ 
THZ 
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VECTA(l) 

VECTA(2)   i 

VECTA(3) 

VECTB(l) 

VECTB(2) 

VECTB(3) 

CALL  CPROD 

TMAT(1,1)  = 

TMAT(1,2)  = 

TMAT(1,3)  = 

TMATTR(1,1) 

TMATTR(1,2) 

TMATTR(1,3) 

TMATTR(2,1) 

TMATTR(2,2) 

TMATTR(2,3^ 

TMATTR(3,1) 

TMATTR(3,2) 

TMATTR(3,3) 


TMAT(2,1) 
TMAT( 2,2) 
TMAT(2,3) 
TMAT(3,1) 
TMAT(3,2) 
TMAT(3,3) 
(VECTA,VECTB,MI1,MJ1,MK1) 
Mil 
MJ1 
MK1 

=  TMAT(1,1) 
=  TMAT(2,1) 
=  TMAT(3,1) 
=  TMAT(1,2) 
=  TMAT(2,2) 
=  TMAT(3,2) 
=  TMAT(1,3) 
=  TMAT(2,3) 
=  TMAT(3,3) 


DO  5  II  =  1,3 
DO  5  J  =1,3 
TEMP  =  0.  0D0 
DO  1  K  =1,3 

TEMP  =  TMAT(I1,K)  * 


1 

CONTINUE 

MATTMP(I1,J)  ■ 

=  TEMP 

5 

CONTINUE 

DO  8    11  = 

1,3 

DO  8    J  = 

1,3 

TEMP  =  0. 

0D0 

DO  7   K 

=  i,: 

3 

TEMP  = 

MATTMP(I1,K) 

7 

CONTINUE 

LIMAT(I,I1 

,J)  = 

TEMP 

8 

CONTINUE 

9 

CONTINUE 

DERIVATIVE 

NO SORT 

230 

CALL  ERRSET 

(208 

,256,-1, 

IMAT(I,K,J)  +  TEMP 


TMATTR(K,J)  +  TEMP 


.1,1) 

CALL  UERSET(LEVELQ,LEVLDQ) 


235 

240 


INITIALIZE  MATRIX  A  AND  B  TO  ZERO 
DO  240  I  =  1,27 
DO  235  J  =  1,27 

MATA(I,J)  =  0.0D0 
CONTINUE 
MATB(I)  =  0.0D0 
CONTINUE 


INPUT  JOINT  EQUATIONS 
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*      JOINT  ZERO  EQUATIONS 
VI  X  (Wl  X  RB/G1) 

RBG1(1)  =  JXO  -  LCOGX(l) 

RBG1(2)  =  JYO  -  LCOGY(l) 

RBG1(3)  =  JZO  -  LCOGZ(l) 

VECTA(l)  =  Wl(l) 

VECTA(2)  =  Wl(2) 

VECTA(3)  =  Wl(3) 
CALL  CPROD(VECTAO,RBGl, MICO, MJCO, MKCO) 

VECTBO(l)  =  MICO 

VECTBO(2)  =  MJCO 

VECTB0(3)  =  MKCO 
CALL  CPROD( VECTAO , VECTBO , MICO , MJCO , MKCO ) 


* 

INPUT  JOINT  EQUATIONS 

* 

JOINT  ZERO  EQUATIONS 

* 

Wl  X  (Wl  X  RB/G1) 

RBGK1)   =  JXO  -  LCOGX(l) 

RBG1(2)   =  JYO  -  LCOGY(l) 

RBG1(3)   =  JZO  -  LCOGZ(l) 

VECTA(l)  =  Wl(l) 

VECTA(2)  =  Wl(2) 

VECTA(3)  =  Wl(3) 

CALL  CPROD(VECTA,RBGl, MICO, MJCO, MKCO) 

VECTB(l)  =  MICO 

VECTB(2)  =  MJCO 

VECTB(3)  =  MKCO 

CALL  CPROD( VECTA , VECTB , MICO , MJCO , MKCO ) 

* 

Wl  X  (Wl  X  RA/G1) 

RAGl(l)  =  JX1  -  LCOGX(l) 

RAG1(2)  =  JY1  -  LCOGY(l) 

RAG1(3)  =  JZ1  -  LCOGZ(l) 

VECTA(l)  =  Wl(l) 

VECTA(2)  =  Wl(2) 

VECTA(3)  =  Wl(3) 

CALL  CPROD  ( VECTA, RAG1 ,MIC1 ,MJC1 ,MKC1) 

VECTB(l)  =  MIC1 

VECTB(2)  =  MJC1 

VECTB(3)  =  MKC1 

CALL  CPROD  (VECTA, VECTB, MIC1 ,MJC1,MKC1) 

* 

W2  X  (W2  X  RB/G2) 

RBG2(1)  =  JX1  -  LCOGX(2) 

RBG2(2)  =  JY1  -  LCOGY(2) 

RBG2(3)  =  JZ1  -  LC0GZ(2) 

VECTA(l)  =  W2(l) 

VECTA(2)  =  W2(2) 

VECTAO)  =  W2(3) 

CALL  CPROD  ( VECTA, RBG2 ,MIC2,MJC2,MKC2) 

VECTB(l)  =  MIC2 

VECTB(2)  =  MJC2 

VECTB(3)  =  MKC2 

CALL  CPROD  (VECTA, VECTB, MIC2 ,MJC2 ,MKC2) 

* 

W2  X  (W2  X  RA/G2) 

RAG2(1)  =  JX2  -  LCOGX(2) 

RAG2(2)  =  JY2  -  LCOGY(2) 

RAG2(3)  =  JZ2  -  LCOGZ(2) 
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* 


VECTA(l)  =  W2(l) 

VECTA(2)  =  W2(2) 

V'ECTA(3)  =  W2(3) 
CALL  CPROD  (VECTA,RAG2,MIC3,MJC3,MKC3) 

VECTB(l)  =  MIC3 

VECTB(2)  =  MJC3 

VECTB(3)  =  MKC3 
CALL  CPR0D(VECTA,VECTB,MIC3,MJC3,MKC3) 
W3  X  (W3  X  RB/G3) 

RBG3(1)  =  JX2  -  LC0GX(3) 

RBG3(2)  =  JY2  -  LC0GY(3) 

RBG3(3)  =  JZ2  -  LC0GZ(3) 

VECTA(l)  =  W3(l) 

VECTA(2)  =  W3(2) 

VECTA(3)  =  W3(3) 
CALL  CPROD  (VECTA,RBG3,MIC4,MJC4,MXC4) 

VECTB(l)  =  MIC4 

VECTB(2)  =  MJC4 

VECTB(3)  =  MKC4 
CALL  CPROD  (VECTA,VECTB,MIC4,MJC4,MXC4) 


■)V 


INERTIA  TRANSFORMATION 


DO  390  I  =  2,  3 


TMAT(2,1) 
TMAT(2,2) 
TMAT(2,3) 
TMAT(3,1) 
TMAT( 3,2) 
TMAT( 3,3: 


-DCOS  ( 
-DSIN  ( 
0.  0D0 
DRCSX(I) 
DRCSY(I) 
DRCSZ(I) 


THZ 
THZ 


VECTA(l)  = 
VECTA(2)  = 
VECTA(3)  = 
VECTB(l)  = 
VECTB(2)  = 
VECTB(3)  = 
CALL  CPROD  ( 
TMAT(1,1)  = 
TMAT(1,2)  = 
TMAT(1,3)  = 
TMATTR(1,1) 
TMATTR(1,2) 
TMATTR(1,3) 
TMATTR(2,1) 
TMATTR(2,2) 
TMATTR(2,3) 
TMATTR(3,1) 
TMATTR(3,2) 
TMATTR(3,3) 


TMAT(2,1) 

TMAT(2,2) 

TMAT(2,3) 

TMAT(3,1) 

TMAT(3,2) 

TMAT(3,3) 

VECTA,VECTB,MI1,MJ1,MX1) 

Mil 

MJ1 

MX1 

=  TMAT(1,1) 

=  TMAT(2,1) 

=  TMAT(3,1) 

=  TMAT(1,2) 

=  TMAT(2,2) 

=  TMAT(3,2) 

=  TMAT(1,3) 

=  TMAT(2,3) 

=  TMAT(3,3) 


370 


DO  375  II  =  1,3 
DO  375  J  =  1,3 
TEMP  =  0.  0D0 
DO  370  K  =  1,3 

TEMP  =  TMATTR(I1,K)  *  LIMAT(I,X,J)  +  TEMP 
CONTINUE 


104 


375 


MATTMP(I1,J)  =  TEMP 
CONTINUE 


DO  380  11  =  1,3 

DO  380  J  =1,3 

TEMP  =  0.  0D0 

DO  378  K  =  1,3 

TEMP  =  MATTMP(I1,K) 

378 

CONTINUE 

NIMAT(I1,J)  =  TEMP 

380 

CONTINUE 

IXXT(I)  =  NIMAT(1,1) 

IXYT(I)  =  NIMAT(1,2) 

IXZT(I)  =  NIMAT(1,3) 

IYYT(I)  =  NIMAT(2,2) 

IYZT(I)  =  NIMAT(2,3) 

IZZT(I)  =  NIMAT(3,3) 

390 

CONTINUE 

*  TMAT(K,J)  +  TEMP 


■A.J..A.J..J, 


-*  ENTER  CONSTANTS  INTO  MATRIX  A  ***** 


** 

LINK  ONE 

* 

SUM  OF  FORCES 

* 

X  DIRECTION 

395 

MATA(1,1)   = 

1.  0D0 

MATA(1,4)   = 

Ml 

MATA(1,10)  = 

-1.  0D0 

MATB(l) 

0.  0D0 

* 

Y  DIRECTION 

MATA(2,2)   = 

1.  ODO 

MATA(2,5)   = 

Ml 

MATA(2,11)  = 

-1.  ODO 

MATB(2) 

0.0D0 

* 

Z  DIRECTION 

MATA(3,3)   = 

1.  ODO 

MATA(3,6)   = 

Ml 

MATA(3,12)  = 

-1.  ODO 

MATB(3) 

-WGl 

* 

EQUATIONS  AT  JOINT  ZERO 

* 

X  DIRECTION 

MATA(4,4)  = 

1.  ODO 

MATA(4,8)  = 

RBG1(3) 

MATA(4,9)  = 

-RBG1(2) 

MATB(4) 

AXO  -  MICO 

* 

Y  DIRECTION 

MATA(5,5)  = 

1.  ODO 

MATA(5,7)  = 

-RBG1(3) 

MATA(5,9)  = 

RBGl(l) 

MATB(5) 

AYO  -  MJCO 

* 

Z  DIRECTION 

MATA(6,6)  = 

l.ODO 

MATA(6,7)  = 

RBG1(2) 

MATA(6,8)  = 

-RBGl(l) 
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* 


Vc 


* 


MATB(6) 

SUM  OF  MOMENTS 

X  DIRECTION 

MATA(7,2)  = 

MATA(7,3)  = 

MATA(7,7)  = 

MATA(7,8)  = 

MATA( 7,9)  = 

MATA(7,11)  = 

MATA(7,12)  = 
MATB(7) 

Y  DIRECTION 

MATA(8,1)  = 

MATA(8,3)  = 

MATA(8,7)  = 

MATA(8,8)  = 

MATA(8,9)  = 

MATA(8,10)  = 

MATA(8,12)  = 
MATB(8) 

Z  DIRECTION 

MATA(9,1)  = 

MATA(9,2)  = 

MATA( 9,7)  = 

MATA(9,8)  = 

MATA(9,9)  = 

MATA(9,10)  = 

MATA(9,11)  = 
MATB(9) 


AZO  -  MKCO 
EQUATIONS 

RBG1(3 
-RBG1(2 
-IXXT(1 

IXYT( 1 

IXZT(1 
-RAG1(3 

RAG1(2 

T1X  -  TOX 


•RBG1(3 
RBG1(1 
IXYT( 1 
■IYYT(1 
IYZT(1 
RAG1(3 
•RAG1(1 
T1Y   - 

RBG1C2 

■RBG1(1 

IXZT(1 

IYZT(1 

■IZZT(1 

-RAG1(2 

RAG1(1 

T1Z    - 


TOY 


TOZ 


**  LINK  TOO 

SUM  OF  FORCES 

X  DIRECTION 

460  MATA(10,10) 

MATA(10,13) 

MATA(10,19) 

MATB(IO) 

*  Y  DIRECTION 

MATA(11,11) 
MATA(11,14) 
MATA(11,20) 
MATB(ll) 

*  Z   DIRECTION 

MATA(12,12) 
MATA(12,15) 
MATA( 12,21) 
MATB(12) 

*  EQUATIONS  AT 

*  X  DIRECTION 

MATA(13,4) 

MATA(13,8) 

MATA(13,9) 

MATA(13,13) 

MATA(13,17) 

MATA(13,18) 


=   1.0D0 
=  M2 
=  -1.  0D0 
=   0.0D0 

=  1.0D0 

=  M2 

=  -1.0D0 

=  0.  ODO 

=   1.  ODO 
=  M2 
=  -1.  ODO 
=  -WG2 
JOINT  ONE 

=  -1.  ODO 
=  -RAG1(3) 
=  RAG1(2) 
=   l.ODO 
=  RBG2(3) 
=  -RBG2(2) 
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MATB(13) 

= 

MIC1  -  MIC2 

*      Y  DIRECTION 

MATA(14,5) 

= 

-1.  0D0 

MATA(14,7) 

= 

RAG1(3) 

MATA(14,9) 

= 

-RAGl(l) 

MATA(14,14) 

= 

l.ODO 

MATA(14,16) 

= 

-RBG2(3) 

MATA(14,18) 

S 

RBG2(1) 

MATB(14) 

= 

MJC1  -  MJC2 

*      Z  DIRECTION 

MATA(15,6) 

= 

-l.ODO 

MATA(15,7) 

= 

-RAG1(2) 

MATA(15,8) 

= 

RAGl(l) 

MATA(15,15) 

= 

1.  ODO 

MATA( 15,16) 

= 

RBG2(2) 

MATA(15,17) 

S 

-RBG2(1) 

MATB(15) 

B 

MKC1  -  MKC2 

*      SUM  OF  MOMENTS 

;  eqi 

*      X  DIRECTION 

MATA(16,11) 

= 

RBG2(3) 

MATA(16,12) 

= 

-RBG2(2) 

MATA(16,16) 

= 

-IXXT(2) 

MATA(16,17) 

= 

IXYT(2) 

MATA(16,18) 

= 

IXZT(2) 

MATA( 16,20) 

= 

-RAG2(3) 

MATA( 16,21) 

b 

RAG2(2) 

MATB(16) 

= 

T2X  -  T1X 

*      Y  DIRECTION 

MATA(17,10) 

= 

-RBG2(3) 

MATA(17,12) 

= 

RBG2(1) 

MATA( 17,16) 

= 

IXYT(2) 

MATA(17,17) 

= 

-IYYT(2) 

MATA(17,18) 

= 

IYZT(2) 

MATA(17,19) 

= 

RAG2(3) 

MATA(17,21) 

= 

-RAG2(1) 

MATB(17) 

= 

T2Y  -  T1Y 

Z  DIRECTION 

MATA(18,10) 

= 

RBG2(2) 

MATA(18,11) 

B 

-RBG2(1) 

MATA(18,16) 

= 

IXZT(2) 

MATA(18,17) 

B 

IYZT(2) 

MATA(18,18) 

= 

-IZZT(2) 

MATA(18,19) 

= 

-RAG2(2) 

MATA( 18,20) 

= 

RAG2(1) 

MATB(18) 

= 

T2Z  -  T1Z 

**     LINK  THREE 

*      SUM  OF  FORCES 

*      X  DIRECTION 

530      MATA(19,19) 

= 

l.ODO 

MATA( 19,22) 

= 

M3 

MATB(19) 

= 

O.ODO 

*      Y  DIRECTION 

MATA(20,20) 

= 

1.  ODO 

MATA(20,23) 

B 

M3 
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MATB(20) 

B 

O.ODO 

* 

Z  DIRECTION 

MATA(21,21) 

= 

1.  ODO 

MATA(21,24) 

= 

M3 

MATB(21)  =  ■ 

•WG3 

* 

EQUATIONS  AT  JOINT  TWO 

* 

X  DIRECTION 

MATA(22,13) 

B 

-1.  ODO 

MATA(22,17) 

= 

-RAG2C3) 

MATA(22,18) 

= 

RAG2(2) 

MATA(22,22) 

= 

1.  ODO 

MATA(22,26) 

= 

RBG3(3) 

MATA(22,27) 

= 

-RBG3(2) 

MATB(22) 

= 

MIC3  -  MIC4 

* 

Y  DIRECTION 

MATA(23,14) 

= 

-l.ODO 

MATA(23,16) 

= 

RAG2(3) 

MATA(23,18) 

s 

-RAG2(1) 

MATA( 23,23) 

= 

l.ODO 

MATA(23,25) 

= 

-RBG3(3) 

MATA( 23,27) 

= 

RBG3(1) 

MATB(23) 

= 

MJC3  -  MJC4 

* 

Z  DIRECTION 

MATA(24,15) 

= 

-1.  ODO 

MATA(24,16) 

s 

-RAG2(2) 

MATA(24,17) 

= 

RAG2(1) 

MATA(24,24) 

= 

1.  ODO 

MATA(24,25) 

= 

RBG3(2) 

MATA(24,26) 

B 

-RBG3(1) 

MATB(24) 

= 

MKC3  -  MKC4 

* 

SUM  OF  MOMENTS  EQUATIONS 

* 

X  DIRECTION 

MATA(25,20) 

B 

RBG3(3) 

MATA(25,21) 

= 

-RBG3(2) 

MATA(25,25) 

= 

-IXXT(3) 

MATA(25,26) 

= 

IXYT(3) 

MATA(25,27) 

= 

IXZT(3) 

MATB(25) 

= 

-T2X 

* 

Y  DIRECTION 

MATA(26,19) 

a 

-RBG3(3) 

MATA(26,21) 

= 

RBG3(1) 

MATA(26,25) 

= 

IXYT(3) 

MATA(26,26) 

= 

-IYYT(3) 

MATA(26,27) 

= 

IYZT(3) 

MATB(26) 

= 

-T2Y 

* 

Z  DIRECTION 

MATA(27,19) 

= 

RBG3(2) 

MATA(27,20) 

= 

-RBG3(1) 

MATA(27,25) 

— 

IXZT(3) 

MATA(27,26) 

= 

IYZT(3) 

MATA(27,27) 

= 

-IZZT(3) 

MATB(27) 

= 

-T2Z 

•icicicicic 

FTRST  T.TMK  T< 

!  r 

inNSTBATNF.Tl  M 

DO  600    I   =  4,9 
DO   595   J  =   1,27 
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MATA(I,J)  =  0.  0D0 

MATA(J,I)  =  0.  ODO 

595 

CONTINUE 

MATB(I)  =  0.  ODO 

600 

CONTINUE 

MATA(4,4)  =  1.  ODO 

MATA(5,5)  =  1.  ODO 

MATA(6,6)  =  1.  ODO 

MATA(7,7)  =  l.ODO 

MATA(8,8)  =  1.  ODO 

MATA(9,9)  =  1.  ODO 

DO  602  I  =  13,17 

DO  597  J  =  1,27 

MATA(I,J)  =  0.  ODO 

MATA(J,I)  =  0.  ODO 

597 

CONTINUE 

MATB(I)  =  O.ODO 

602 

CONTINUE 

MATA(13,13)  =  l.ODO 

MATA(14,14)  =  1.  ODO 

MATA(15,15)  =  1.  ODO 

MATA(16,16)  =  l.ODO 

MATA(17,17)  =  l.ODO 

605 

DO  610  J  =  1,27 

MATA(18,J)  =  0.  ODO 

MATA(27,J)  =  0.  ODO 

610 

CONTINUE 

MATA(18,9)   =  -l.ODO 

MATA(18,18)  =   1.  ODO 

MATB(18)     =   O.ODO 

MATA(27,9)   =  -1. ODO 

MATA(27,27)  =   1.  ODO 

MATB(27)     =   O.ODO 

* 

CALL  EQUATION  SOLVER  PROG 

620 

CALL  LEQT2F(MATA,M,N, 

IF  (IER.  EQ.  0)  THEN 

GOTO  640 

ELSE 

WRITE  (*,624)  IER 
624        FORMAT  (15) 

DO  635   I  =  1,  27 

WRITE  (*,627)  I 
627        FORMAT  (17) 

DO  631   J  =  1,  27,  3 

WRITE  (*,630)  J,MATA(I,J),J+l,MATA(I,J+l),J+2,MATA(I,J+2) 

630  FORMAT  ( 15 ,F13. 5 , 15 ,F13. 5,15 ,F13. 5) 

631  CONTINUE 

WRITE  (-,633)  I,MATB(I) 
633        FORMAT  (I3,F15.5) 
635        CONTINUE 

CALL  ENDJOB 

END  IF 
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?w«v 


FIND  LCOGX,LCOGY,LCOGZ,THETA  VALUES ,WX,WY,WZ 
MODIFIED  BY  R. M. VERBOS 


* 

JOINT  ZERO 

640 

FXO  =  MATB(l) 

FYO  =  MATB(2) 

FZO  =  MATB(3) 

* 

LINK  ONE 

* 

SINCE  LINK1  JS  CONSTRAIN  TO  ROT 

AX1  =  0.  ODO 

AY1  =  0.  ODO 

AZ1  =  0.  ODO 

*660 

AX1       =  MATB(4) 

Vf 

VELX1     =  INTGRL(0. ,AX1) 

* 

LC0GX1    =  INTGRL(X1,VELX1) 

* 

LCOGX(l)   =  LC0GX1 

* 

AY1       =  MATB(5) 

* 

VELY1     =  INTGRLCO. ,AY1) 

* 

LC0GY1    =  INTGRL(Y1,VELY1) 

* 

LCOGY(l)   =  LC0GY1 

A 

AZ1       =  MATB(6) 

* 

VELZ1     =  INTGRL(0.  ,AZ1) 

■sV 

LC0GZ1    =  INTGRL(Z1,VELZ1) 

* 

LCOGZ(l)   =  LC0GZ1 

WD IX      =  MATB(7) 

W1X       =  INTGRLCO. ,WD1X) 

WDX(l)    =  WD1X 

wicn          =  W1X 

WD1Y      =  MATB(8) 

WIY       =  INTGRL(0. ,WD1Y) 

WDY(l)    =  WD1Y 

Wl(2)     =  WIY 

WD1Z      =  MATB(9) 

WIZ       =  INTGRLCO. ,WD1Z) 

WDZ(l)    =  WD1Z 

Wl(3)     =  WIZ 

Mric 

ADDED  BY  R. M. VERBOS 

685 

THZ       =  INTGRLCO.  ,W1Z) 

THZANG    =  THZ  *  RADEG 

COSTHZ    =  DCOS(THZ) 

SINTHZ    =  DSIN(THZ) 

AROUND  THE  Z  ONLY 


* 


IF  THE  1ST  LINK  IS  CONSTRAIN  TO  ROTATE  IN  THE  Z  DIRECTION  ONLY 
THE  DIRECTION  COSINE  AND  DIRECTION  COSINE  ANGLES  ARE  CONSTANT 
AND  DO  NOT  NEED  TO  BE  CALCULATED 

CALC  DIRECTIONAL  COSINES  FOR  LINK  ONE 


*695 
*700 

it 


LNC0G1  =  DSQRT  (  LC0GX1*LC0GX1  +  LC0GY1*LC0GY1  +. 

LC0GZ1*LC0GZ1  ) 
DRCSX(l)   =  LC0GX1  /  LNC0G1 
DRCSYC1)   =  LC0GY1  /  LNC0G1 
DRCSZ(l)   =  LC0GZ1  /  LNC0G1 
AMP  =  DSQRT(DRCSX(1)*DRCSX(1)+DRCSYU)*DRCSY(1)+.  . 
DRCSZ(1)*DRCSZ(1)) 
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* 

* 
*720 

* 
* 

*730 
* 

* 


DRCSX(l)  =  DRCSX(1)/AMP 

DRCSY(l)  =  DRCSY(1)/AMP 

DRCSZ(l)  =  DRCSZ(1)/AMP 

DRCANX(l)  =  DAC0S(DRCSX(1))  *  RADEG 

DRCANY(l)  =  DACOS(DRCSY(l))  *  RADEG 

DRCANZ(l)  =  DACOS(DRCSZ(l))  *  RADEG 

JOINT  LOCATION 

JX1  =  LINKL1  *  DRCSX(l) 

JY1   =  LINKL1  *  DRCSY(l) 

JZ1   =  LINKL1  *  DRCSZ(l) 


*      JOINT  ONE 

735  FX1  =  MATB(IO) 
FY1  =  MATB(ll) 
FZ1  =  MATB(12) 

AX2  =  0.  ODO 
AY2  =  0.  ODO 
AZ2  =  O.ODO 


Vf>V 

740 
* 

■k 
* 

* 
* 


LINK  TWO 
AX2 
VELX2 
LC0GX2 
LC0GX(2) 
AY2 
VELY2 
LC0GY2 
LC0GY(2) 
AZ2 
VELZ2 
LC0GZ2 
LC0GZ(2) 
WD2X 
W2X 
WDX(2) 
W2(l) 
WD2Y 
W2Y 
WDY(2) 
W2(2) 
WD2Z 
W2Z 
WDZ(2) 
W2(3) 


MATB(13) 

INTGRL(0. 

INTGRL(X2 

LC0GX2 

MATB(14) 

INTGRL(0. 

INTGRL(Y2 

LC0GY2 

MATB(15) 

INTGRL(0. 

INTGRL(Z2 

LC0GZ2 

MATB(16) 

INTGRL(0.  ,WD2X) 

WD2X 

W2X 

MATB(17) 

INTGRL( 0 

WD2Y 


,AX2) 
,VELX2) 


,AY2) 
,VELY2) 


,AZ2) 
,VELZ2) 


,WD2Y) 


=  W2Y 
=  MATB(18) 
=  INTGRL(0. 
=  WD2Z 
=  W2Z 


,WD2Z) 


* 

*90 

it 
* 
* 


GET  THE  DIRECTION  COSINES  FOR  THE  LINK  TWO 
DRCSX(2)   =  (LC0GX2  -  JX1)  /  LNC0G2 
DRCSY(2)   =  (LC0GY2  -  JY1)  /  LNC0G2 
DRCSZ(2)   =  (LC0GZ2  -  JZ1)  /  LNC0G2 
AMP  =  DSQRT(DRCSX(2)*DRCSX(2)+DRCSY(2)*DRCSY(2)+. 

DRCSZ(2)*DRCSZ(2)) 
DRCSX(2)  =  DRCSX(2)/AMP 
DRCSY(2)  =  DRCSY(2)/AMP 
DRCSZ(2)  =  DRCSZ(2)/AMP 
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* 


DRCANX(2)  =  DAC0S(DRCSX(2))  *  RADEG 
DRCANY(2)  =  DAC0S(DRCSY(2))  *  RADEG 
DRCANZ(2)  =  DAC0S(DRCSZ(2))  *  RADEG 


*  JOINT  LOCATION 

800  JX2  =  JX1  +  LINKL2  *  DRCSX(2) 
JY2  =  JY1  +  LINKL2  *  DRCSY(2) 
JZ2  =  JZ1  +  LINKL2  *  DRCSZ(2) 

*  JOINT  TWO 

805  FX2  =  MATB(19) 
FY2  =  MATB(20) 
FZ2  =  MATB(21) 


**     LINK  THREE 
812      AX3 

VELX3 

LCOGX3 

LCOGX(3) 

AY3 

VELY3 

LCOGY3 

LCOGY(3) 

AZ3 

VELZ3 

LCOGZ3 

LCOGZ(3) 

WD3X 

W3X 

VDX(3) 

W3(l) 

WD3Y 

W3Y 

WDY(3) 

W3(2) 

WD3Z 

W3Z 

WDZ(3) 

W3(3) 


,AX3) 

,VELX3) 


iIC,AY3) 
,VELY3) 


,AZ3) 
,VELZ3) 


=  MATB(22) 

=  INTGRL(0. 

=  INTGRL(X3 

=  LCOGX3 

=  MATB(23) 

=  INTGRL(V3: 

=  INTGRL(Y3 

=  LCOGY3 

=  MATB(24) 

=  INTGRL(0. 

=  INTGRL(Z3 

=  LCOGZ3 

=  MATB(25) 

=  INTGRL(W3IC,WD3X) 

=  WD3X 

=  W3X 

=  MATB(26) 

=  INTGRL(0.  ,WD3Y) 

=  WD3Y 

=  W3Y 

=  MATB(27) 

=  INTGRL(0.  ,WD3Z) 

=  WD3Z 

=  W3Z 


*  CALC  DIRECTIONAL  COSINES  FOR  LINK  THREE 
845      DRCSX(3)  =  (LCOGX3  -  JX2)  /  LNCOG3 
DRCSY(3)  =  (LCOGY3  -  JY2)  /  LNCOG3 
DRCSZ(3)  =  (LCOGZ3  -  JZ2)  /  LNCOG3 
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AMP  =  DSQRT(DRCSX(3)*DRCSX(3)+DRCSY(3)*DRCSY(3)+. 

DRCSZ(3)*DRCSZ(3)) 
DRCSX(3)  =  DRCSX(3)/AMP 
DRCSY(3)  =  DRCSY(3)/AMP 
DRCSZ(3)  =  DRCSZ(3)/AMP 
DRCANX(3)  =  DACOS(DRCSX(3))  *  RADEG 
DRCANY(3)  =  DACOS(DRCSY(3))  *  RADEG 
DRCANZ(3)  =  DACOS(DRCSZ(3))  *  RADEG 


*   TIP  LOCATION 
875      TIPX  =  JX2  +  LINKL3 
TIPY  =  JY2  +  LINKL3 


*  DRCSX(3) 

*  DRCSY(3) 
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TIPZ  =  JZ2  +  LINKL3  *  DRCSZ(3) 


* 

880 


FIND  THE 
ANG23X  = 
ANG23Y  = 
ANG23Z  = 
ANG12X  = 
ANG12Y  = 
ANG12Z  = 


ANGLE  BETWEEN  THE  LIMKS 


DRCANX(2) 
DRCANY(2) 
DRCANZ(2) 
DRCANX(l) 
DRCANY(l) 
DRCANZ(l) 


DRCANX(3) 
DRCANY(3) 
DRCANZ(3) 
DRCANX(2) 
DRCANY(2) 
DRCANZ(2) 


DYNAMIC 

N0S0RT 

*****   INPUT  TORQUE  AT 
T2FNC  =  (13. 
T2  =  T2FNC 
T2X  =  T2 


JOINTS 
5  *  SIN 


(PI  /  (30.0)  *  TIME))  *  RCNFAC 


END 
STOP 


FORTRAN 


* 


SUBROUTINE  TO  COMPUTE  THE  CROSS  PRODUCT  OF  TWO  VECTORS 
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SUBROUTINE  CPROD( VECTA , VECTB , MI , M J , MK) 
IMPLICIT  REAL*8  (A-Z) 
DIMENSION  VECTA(3),VECTB(3) 
MI  =  VECTA(2)  *  VECTB(3)  -  VECTA(3) 
MJ  =  VECTA(3)  *  VECTB(l)  -  VECTA(l) 
MK  =  VECTA(l)  *  VECTB(2)  -  VECTA(2) 

RETURN 

END 


* 
* 


VECTB(2) 
VECTB(3) 
VECTB(l) 
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APPENDIX  E 

BASIC  OPERATING  INSTRUCTIONS  FOR  THE  NAVAL 
POSTGRADUATE  SCHOOL  RIGID  MANIPULATOR  TEST  BED 

A.      SYSTEM  FAMILIARIZATION 

Before  operating  the  manipulator  test  bed,  you  should  ensure  you 
are  familiar  with  its  components  and  basic  procedures  of  operation.  If 
you  are  already  familiar  with  the  components  of  the  manipulator  and 
their  set-up,  you  will  need  to  review  this  section  to  operate  the 
hardware. 

The  test  bed  consists  of  four  major  sub-systems:  the  Neptune  II 
hydraulically  operated  manipulator;  an  IBM  PC -AT  used  to  control  the 
manipulator  and  gather  data  (at  the  present  time);  an  IBM  PC-XT  (not 
in  the  system  at  present);  and  an  Electronic  Control  Panel  used  in 
switching,  data  gathering,  and  interfacing. 

1.     The  Manipulator 

The  manipulator  (Figure  E-l),  being  a  hydraulic  mechanism, 
is  subject  to  leakage.  It  is  advisable  to  wear  old  clothing  while  in  the 
lab.  The  operating  pressure  of  the  system  is  around  100  psi  and  a 
minor  leak  may  spray  anywhere  in  the  lab. 

The  source  of  the  hydraulic  fluid  is  the  pump  cabinet  against 
the  forward  side  of  the  wall  in  front  of  the  computers.  This  cabinet 
contains  a  pump  which  is  operated  from  the  ECP,  a  reservoir  which  is 
underneath  the  pump,  and  an  accumulator  that  is  alongside  of  the 
pump  (Figure  E-2). 
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Figure  E-l.   Manipulator 
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SUPPLY  TO 
ACCUMULATOR 


SUPPLY  HOSE  TO  ROBOT 


RETURN  HOSE 
TO  ROBOT 


Figure  E-2.   Pump  Cabinet 
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The  reservoir  has  a  fill  hole  on  the  top.  It  is  necessary  to 
check  this  before  operating  the  system  and  ensure  that  the  level  is 
around  half  when  the  system  is  not  in  operation.  You  will  also  notice  a 
pressure  gage  which  reads  the  pressure  in  the  accumulator.  This 
pressure  should  be  between  80  and  100  psi  and  may  be  increased  by 
pressurizing  the  bladder  in  the  accumulator  to  a  higher  pressure. 

If  you  look  closely  at  the  accumulator,  you  will  notice  a 
solenoid  valve  at  the  outlet.  This  valve  has  been  installed  because  of 
previous  problems  and  will  cut  off  all  hydraulic  flow  to  the  manipulator 
when  it  is  shut  from  the  ECP. 

Underneath  the  manipulator  itself  is  a  set  of  Electronic 
Printed  Circuit  Boards  (EPCB).  These  boards  control  the  opening  and 
shutting  of  the  solenoid  values  to  the  joint  pistons  (see  Ref.  9  for  more 
information).  The  important  thing  to  know  about  these  boards  is  that 
you  should  avoid  getting  hydraulic  fluid  on  them. 
2.     The  Computers 

At  present,  the  IBM-AT  is  the  only  computer  hooked  up  in 
the  system.  It  is  easy  to  follow  because  it  has  been  set  up  with  a  menu- 
based  operating  system.  The  power  to  the  AT  is  under  the  right  side  of 
the  computer  stand.  The  first  thing  that  will  happen  when  you  turn 
the  power  on  is  that  the  AT  will  go  thru  a  cold-start  boot-up,  which 
may  take  a  minute  or  two.  You  will  then  notice  a  listing  of  subdirecto- 
ries on  the  screen.  Most  of  these  subdirectories  are  of  little  use 
because  the  files  are  proprietary  in  nature  and  the  author  has  long 
since  transferred  away  from  NPS,  but  feel  free  to  skim  through  them. 
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For  the  initial  running  of  the  manipulator,  you  will  want  to 
select  the  BATCHES  subdirectory.  Once  you  are  in  this  subdirectory, 
you  will  select  the  NEPTROLA  file  to  run  the  manipulator.  This  file 
contains  the  control  program  for  operating  the  manipulator  in  the 
solenoid  mode.  The  program  is  a  useful  starting  point  for  examining 
the  range  of  the  arm  motion. 

NEPTROLA  is  a  menu-driven  set-up  that  asks  various  ques- 
tions to  get  the  system  in  the  proper  operating  configuration.  The 
actual  operation  will  be  discussed  in  the  operation  section  of  this 
appendix. 

3.     Electronic  Control  Panel  (ECP) 

The  ECP  is  located  between  the  IBM-AT  and  the  IBM-XT  and 
is  the  heart  of  the  control  of  the  manipulator.  The  ECP  contains  sev- 
eral interface  boards  which  should  not  be  tampered  with  without 
talking  to  Tom  Cristian  of  the  Mechanical  Engineering  technical  staff. 
The  front  of  the  panel  is  divided  into  four  sections  (Figure  E-3).  Start- 
ing at  the  top,  the  first  sub-panel  consists  of  two  sets  of  electrical 
jacks.  The  first  six  from  the  left  are  connected  to  the  position  data  of 
the  interface  boards  and  will  provide  joint  0  thru  5  position  output. 
The  last  two  jacks  are  for  servo  operation  which  is  no  longer  con- 
nected (more  information  on  servo  operation  is  available  in  Ref.  13). 
The  next  panel  down  was  for  control  of  the  servovalves;  again,  more 
information  is  available  in  Reference  13.  The  next  panel  contains  a 
power  switch  which  selects  power  to  the  panel  and  electrical  jacks  to 
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Figure  E-3.    Electronic  Control  Panel  (ECP) 


119 


monitor  the  pressure  transducer  output  for  torque  calculations.  Pres- 
sure transducers  have  been  installed  on  joints  1  and  2.  The  electrical 
jacks  are  wired  so  that  PI  and  P2  are  connected  to  joint  1  transduc- 
ers, P3  and  P4  are  connected  to  joint  2  transducers,  and  P5  and  P6 
are  available  for  further  connections. 

The  bottom  panel  is  the  power  panel  and  consists  of  seven 
switches  and  three  electrical  jacks.  The  switches,  from  left  to  right, 
are  as  follows: 

•  Electronic  Control  Power,  which  supplies  the  electricity  to  the 
interface  board  at  the  back  of  the  panel. 

•  Solenoid   Current  Switch,   which  must  be  "ON"   to  operate  the 
manipulator  in  the  solenoid  mode. 

•  Servo  Master  Switches  (3),  which  are  three-position  switches: 

-  (DOWN)  is  for  selecting  servovalve  control  from  the  AT 

-  (MIDDLE)  is  for  operating  in  the  solenoid  mode 

-  (UP)  is  for  for  operating  the  servovalves  from  an  auxiliary  signal 
which  is  connected  to  the  three  electrical  jacks  above  the 
switches. 

•  Extra  switch. 

•  (Rightmost)  Pump  Power/ Solenoid  Valve  Control  Switch.  This  is  a 
three-position  switch: 

-  (UP)  the  pump  is  in  the  run  mode  and  controlled  by  pressure 

-  (MIDDLE)  the  pump  is  off  and  the  solenoid  valve  is  open 

-  (DOWN)  the  pump  is  off  and  the  solenoid  valve  has  power  to  it 
and  is  shut  (this  is  an  emergency  cut-off  position). 


120 


B.  OPERATING  THE  ARM 
1.  The  Solenoid  Mode 

Following  is  a  step-by-step  procedure  for  operating  the  Nep- 
tune II  manipulator  in  the  solenoid  mode: 

STEP  1 

Check  the  accumulator  to  ensure  that  there  is  sufficient  fluid  to 
operate  the  manipulator.  If  the  level  is  low,  add  a  50/50  mix  of 
Hydro  Lube  and  water  until  the  level  is  between  one-half  and 
three-quarters  (DO  NOT  FILL). 

STEP  2 

Check  the  accumulator  pressure  to  ensure  it  is  between  80  and 
100  psi.  If  the  pressure  is  low  or  high,  bleed  or  add  air  through 
the  hose  connection  as  necessary. 

STEP  3 

From  the  ECP  turn  the  following  switches  to  the  position  referred 
to: 

-  Electronic  Power  Switch  ON  (UP) 

-  Solenoid  Current  Switch  ON  (UP) 

-  Servo  Master  Switches  OFF  (MIDDLE) 

-  Pump  Power  Switch  ON  (UP) 

STEP  4 

Now  turn  the  IBM-AT  computer  on. 

STEP  5 

Select  the  BATCHES  subdirectory. 

STEP  6 

Select  the  NEPTROLA  program  and  answer  "yes"  (Y)  to  the  ques- 
tion whether  you  want  to  operate  without  ADC.  This  will  allow  you 
to  operate  from  the  keyboard.  The  next  question  will  give  several 
options— pick  the  keyboard  option.  Follow  the  given  computer 
menu  and  vary  the  joint  positions  as  desired. 
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STEP  7 

To  leave  the  NEPTROLA  program,  hit  ESC,  then  hit  the  control 
and  break  key  at  the  same  time.  After  this,  type  "SYSTEM"  and 
you  will  be  returned  to  the  BATCHES  subdirectory  menu. 

2.     Servovalve  Operation 

At  present,  the  servo  valves  have  been  removed  from  the  sys- 
tem but  the  software  and  wiring  are  still  available.  When  the  servo- 
valves  are  reinstalled,  refer  to  [Ref.  13]  for  operating  procedures. 
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